Speed profile working
easyspin.cpp@0:cba942f8172a, 2015-07-07 (annotated)
- Committer:
- julientiron
- Date:
- Tue Jul 07 20:35:36 2015 +0000
- Revision:
- 0:cba942f8172a
- Child:
- 1:9efe863db15e
run function is working;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
julientiron | 0:cba942f8172a | 1 | /******************************************************//** |
julientiron | 0:cba942f8172a | 2 | * @file easyspin.cpp |
julientiron | 0:cba942f8172a | 3 | * @version V1.0 |
julientiron | 0:cba942f8172a | 4 | * @date June 29, 2015 |
julientiron | 0:cba942f8172a | 5 | * @brief easyspin library for mbed |
julientiron | 0:cba942f8172a | 6 | * |
julientiron | 0:cba942f8172a | 7 | * This file is free software; you can redistribute it and/or modify |
julientiron | 0:cba942f8172a | 8 | * it under the terms of either the GNU General Public License version 2 |
julientiron | 0:cba942f8172a | 9 | * or the GNU Lesser General Public License version 2.1, both as |
julientiron | 0:cba942f8172a | 10 | * published by the Free Software Foundation. |
julientiron | 0:cba942f8172a | 11 | **********************************************************/ |
julientiron | 0:cba942f8172a | 12 | |
julientiron | 0:cba942f8172a | 13 | #include "easyspin.h" |
julientiron | 0:cba942f8172a | 14 | #include "mbed.h" |
julientiron | 0:cba942f8172a | 15 | |
julientiron | 0:cba942f8172a | 16 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 17 | /// Log buffer |
julientiron | 0:cba942f8172a | 18 | char EasyspinStrOut[DEBUG_BUFFER_SIZE]; |
julientiron | 0:cba942f8172a | 19 | #endif |
julientiron | 0:cba942f8172a | 20 | |
julientiron | 0:cba942f8172a | 21 | const uint16_t Easyspin::prescalerArrayTimer0_1[PRESCALER_ARRAY_TIMER0_1_SIZE] = { 0, 1, 8, 64, 256, 1024}; |
julientiron | 0:cba942f8172a | 22 | const uint16_t Easyspin::prescalerArrayTimer2[PRESCALER_ARRAY_TIMER2_SIZE] = {0, 1, 8, 32, 64, 128, 256, 1024}; |
julientiron | 0:cba942f8172a | 23 | volatile void (*Easyspin::flagInterruptCallback)(void); |
julientiron | 0:cba942f8172a | 24 | volatile uint8_t Easyspin::numberOfShields; |
julientiron | 0:cba942f8172a | 25 | uint8_t Easyspin::spiTxBursts[Easyspin_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_SHIELDS]; |
julientiron | 0:cba942f8172a | 26 | uint8_t Easyspin::spiRxBursts[Easyspin_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_SHIELDS]; |
julientiron | 0:cba942f8172a | 27 | volatile bool Easyspin::spiPreemtionByIsr = false; |
julientiron | 0:cba942f8172a | 28 | volatile bool Easyspin::isrFlag = false; |
julientiron | 0:cba942f8172a | 29 | volatile class Easyspin* Easyspin::instancePtr = NULL; |
julientiron | 0:cba942f8172a | 30 | |
julientiron | 0:cba942f8172a | 31 | /******************************************************//** |
julientiron | 0:cba942f8172a | 32 | * @brief Constructor |
julientiron | 0:cba942f8172a | 33 | * @param None |
julientiron | 0:cba942f8172a | 34 | * @retval None |
julientiron | 0:cba942f8172a | 35 | **********************************************************/ |
julientiron | 0:cba942f8172a | 36 | Easyspin::Easyspin():reset(Easyspin_Reset_Pin), dir1(Easyspin_DIR_1_Pin), dir2(Easyspin_DIR_2_Pin), |
julientiron | 0:cba942f8172a | 37 | dir3(Easyspin_DIR_3_Pin), CS(Easyspin_CS_Pin), flag(Easyspin_FLAG_Pin), pwm1(Easyspin_PWM_1_Pin), |
julientiron | 0:cba942f8172a | 38 | pwm2(Easyspin_PWM_2_Pin), pwm3(Easyspin_PWM_3_Pin), spi(Easyspin_MOSI_Pin, Easyspin_MISO_Pin, Easyspin_SCK_Pin) |
julientiron | 0:cba942f8172a | 39 | { |
julientiron | 0:cba942f8172a | 40 | uint8_t i; |
julientiron | 0:cba942f8172a | 41 | for (i = 0; i < MAX_NUMBER_OF_SHIELDS; i++) |
julientiron | 0:cba942f8172a | 42 | { |
julientiron | 0:cba942f8172a | 43 | shieldPrm[i].motionState = INACTIVE; |
julientiron | 0:cba942f8172a | 44 | shieldPrm[i].commandExecuted = NO_CMD; |
julientiron | 0:cba942f8172a | 45 | shieldPrm[i].stepsToTake = MAX_STEPS; |
julientiron | 0:cba942f8172a | 46 | } |
julientiron | 0:cba942f8172a | 47 | instancePtr = this; |
julientiron | 0:cba942f8172a | 48 | } |
julientiron | 0:cba942f8172a | 49 | |
julientiron | 0:cba942f8172a | 50 | /******************************************************//** |
julientiron | 0:cba942f8172a | 51 | * @brief Attaches a user callback to the flag Interrupt |
julientiron | 0:cba942f8172a | 52 | * The call back will be then called each time the status |
julientiron | 0:cba942f8172a | 53 | * flag pin will be pulled down due to the occurrence of |
julientiron | 0:cba942f8172a | 54 | * a programmed alarms ( OCD, thermal pre-warning or |
julientiron | 0:cba942f8172a | 55 | * shutdown, UVLO, wrong command, non-performable command) |
julientiron | 0:cba942f8172a | 56 | * @param[in] callback Name of the callback to attach |
julientiron | 0:cba942f8172a | 57 | * to the Flag Interrupt |
julientiron | 0:cba942f8172a | 58 | * @retval None |
julientiron | 0:cba942f8172a | 59 | **********************************************************/ |
julientiron | 0:cba942f8172a | 60 | void Easyspin::AttachFlagInterrupt(void (*callback)(void)) |
julientiron | 0:cba942f8172a | 61 | { |
julientiron | 0:cba942f8172a | 62 | flagInterruptCallback = (volatile void (*)())callback; |
julientiron | 0:cba942f8172a | 63 | } |
julientiron | 0:cba942f8172a | 64 | |
julientiron | 0:cba942f8172a | 65 | /******************************************************//** |
julientiron | 0:cba942f8172a | 66 | * @brief Starts the Easyspin library |
julientiron | 0:cba942f8172a | 67 | * @param[in] nbShields Number of Easyspin shields to use (from 1 to 3) |
julientiron | 0:cba942f8172a | 68 | * @retval None |
julientiron | 0:cba942f8172a | 69 | **********************************************************/ |
julientiron | 0:cba942f8172a | 70 | void Easyspin::Begin(uint8_t nbShields) |
julientiron | 0:cba942f8172a | 71 | { |
julientiron | 0:cba942f8172a | 72 | numberOfShields = nbShields; |
julientiron | 0:cba942f8172a | 73 | |
julientiron | 0:cba942f8172a | 74 | // start the SPI library: |
julientiron | 0:cba942f8172a | 75 | //SPI.begin(); |
julientiron | 0:cba942f8172a | 76 | //SPI.setBitOrder(MSBFIRST); |
julientiron | 0:cba942f8172a | 77 | spi.format(8, 3); |
julientiron | 0:cba942f8172a | 78 | //SPI.setClockDivider(SPI_CLOCK_DIV4); |
julientiron | 0:cba942f8172a | 79 | |
julientiron | 0:cba942f8172a | 80 | // flag pin |
julientiron | 0:cba942f8172a | 81 | //pinMode(Easyspin_FLAG_Pin, INPUT_PULLUP); |
julientiron | 0:cba942f8172a | 82 | flag.fall(&FlagInterruptHandler); |
julientiron | 0:cba942f8172a | 83 | |
julientiron | 0:cba942f8172a | 84 | //reset pin |
julientiron | 0:cba942f8172a | 85 | //pinMode(Easyspin_Reset_Pin, OUTPUT); |
julientiron | 0:cba942f8172a | 86 | |
julientiron | 0:cba942f8172a | 87 | switch (nbShields) |
julientiron | 0:cba942f8172a | 88 | { |
julientiron | 0:cba942f8172a | 89 | case 3: |
julientiron | 0:cba942f8172a | 90 | //pinMode(Easyspin_DIR_3_Pin, OUTPUT); |
julientiron | 0:cba942f8172a | 91 | //pinMode(Easyspin_PWM_3_Pin, OUTPUT); |
julientiron | 0:cba942f8172a | 92 | PwmInit(2); |
julientiron | 0:cba942f8172a | 93 | case 2: |
julientiron | 0:cba942f8172a | 94 | //pinMode(Easyspin_DIR_2_Pin, OUTPUT); |
julientiron | 0:cba942f8172a | 95 | //pinMode(Easyspin_PWM_2_Pin, OUTPUT); |
julientiron | 0:cba942f8172a | 96 | PwmInit(1); |
julientiron | 0:cba942f8172a | 97 | case 1: |
julientiron | 0:cba942f8172a | 98 | //pinMode(Easyspin_DIR_1_Pin, OUTPUT); |
julientiron | 0:cba942f8172a | 99 | //pinMode(Easyspin_PWM_1_Pin, OUTPUT); |
julientiron | 0:cba942f8172a | 100 | PwmInit(0); |
julientiron | 0:cba942f8172a | 101 | default: |
julientiron | 0:cba942f8172a | 102 | ; |
julientiron | 0:cba942f8172a | 103 | } |
julientiron | 0:cba942f8172a | 104 | |
julientiron | 0:cba942f8172a | 105 | /* Standby-reset deactivation */ |
julientiron | 0:cba942f8172a | 106 | ReleaseReset(); |
julientiron | 0:cba942f8172a | 107 | |
julientiron | 0:cba942f8172a | 108 | /* Set all registers and context variables to the predefined values from Easyspin_target_config.h */ |
julientiron | 0:cba942f8172a | 109 | SetShieldParamsToPredefinedValues(); |
julientiron | 0:cba942f8172a | 110 | |
julientiron | 0:cba942f8172a | 111 | /* Disable Easyspin powerstage */ |
julientiron | 0:cba942f8172a | 112 | for (uint32_t i = 0; i < nbShields; i++) |
julientiron | 0:cba942f8172a | 113 | { |
julientiron | 0:cba942f8172a | 114 | CmdDisable(i); |
julientiron | 0:cba942f8172a | 115 | /* Get Status to clear flags after start up */ |
julientiron | 0:cba942f8172a | 116 | CmdGetStatus(i); |
julientiron | 0:cba942f8172a | 117 | } |
julientiron | 0:cba942f8172a | 118 | } |
julientiron | 0:cba942f8172a | 119 | |
julientiron | 0:cba942f8172a | 120 | /******************************************************//** |
julientiron | 0:cba942f8172a | 121 | * @brief Returns the acceleration of the specified shield |
julientiron | 0:cba942f8172a | 122 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 123 | * @retval Acceleration in pps^2 |
julientiron | 0:cba942f8172a | 124 | **********************************************************/ |
julientiron | 0:cba942f8172a | 125 | uint16_t Easyspin::GetAcceleration(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 126 | { |
julientiron | 0:cba942f8172a | 127 | return (shieldPrm[shieldId].acceleration); |
julientiron | 0:cba942f8172a | 128 | } |
julientiron | 0:cba942f8172a | 129 | |
julientiron | 0:cba942f8172a | 130 | /******************************************************//** |
julientiron | 0:cba942f8172a | 131 | * @brief Returns the current speed of the specified shield |
julientiron | 0:cba942f8172a | 132 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 133 | * @retval Speed in pps |
julientiron | 0:cba942f8172a | 134 | **********************************************************/ |
julientiron | 0:cba942f8172a | 135 | uint16_t Easyspin::GetCurrentSpeed(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 136 | { |
julientiron | 0:cba942f8172a | 137 | return shieldPrm[shieldId].speed; |
julientiron | 0:cba942f8172a | 138 | } |
julientiron | 0:cba942f8172a | 139 | |
julientiron | 0:cba942f8172a | 140 | /******************************************************//** |
julientiron | 0:cba942f8172a | 141 | * @brief Returns the deceleration of the specified shield |
julientiron | 0:cba942f8172a | 142 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 143 | * @retval Deceleration in pps^2 |
julientiron | 0:cba942f8172a | 144 | **********************************************************/ |
julientiron | 0:cba942f8172a | 145 | uint16_t Easyspin::GetDeceleration(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 146 | { |
julientiron | 0:cba942f8172a | 147 | return (shieldPrm[shieldId].deceleration); |
julientiron | 0:cba942f8172a | 148 | } |
julientiron | 0:cba942f8172a | 149 | |
julientiron | 0:cba942f8172a | 150 | /******************************************************//** |
julientiron | 0:cba942f8172a | 151 | * @brief Returns the FW version of the library |
julientiron | 0:cba942f8172a | 152 | * @param None |
julientiron | 0:cba942f8172a | 153 | * @retval Easyspin_FW_VERSION |
julientiron | 0:cba942f8172a | 154 | **********************************************************/ |
julientiron | 0:cba942f8172a | 155 | uint8_t Easyspin::GetFwVersion(void) |
julientiron | 0:cba942f8172a | 156 | { |
julientiron | 0:cba942f8172a | 157 | return (Easyspin_FW_VERSION); |
julientiron | 0:cba942f8172a | 158 | } |
julientiron | 0:cba942f8172a | 159 | |
julientiron | 0:cba942f8172a | 160 | /******************************************************//** |
julientiron | 0:cba942f8172a | 161 | * @brief Returns the mark position of the specified shield |
julientiron | 0:cba942f8172a | 162 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 163 | * @retval Mark register value converted in a 32b signed integer |
julientiron | 0:cba942f8172a | 164 | **********************************************************/ |
julientiron | 0:cba942f8172a | 165 | int32_t Easyspin::GetMark(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 166 | { |
julientiron | 0:cba942f8172a | 167 | return ConvertPosition(CmdGetParam(shieldId,Easyspin_MARK)); |
julientiron | 0:cba942f8172a | 168 | } |
julientiron | 0:cba942f8172a | 169 | |
julientiron | 0:cba942f8172a | 170 | /******************************************************//** |
julientiron | 0:cba942f8172a | 171 | * @brief Returns the max speed of the specified shield |
julientiron | 0:cba942f8172a | 172 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 173 | * @retval maxSpeed in pps |
julientiron | 0:cba942f8172a | 174 | **********************************************************/ |
julientiron | 0:cba942f8172a | 175 | uint16_t Easyspin::GetMaxSpeed(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 176 | { |
julientiron | 0:cba942f8172a | 177 | return (shieldPrm[shieldId].maxSpeed); |
julientiron | 0:cba942f8172a | 178 | } |
julientiron | 0:cba942f8172a | 179 | |
julientiron | 0:cba942f8172a | 180 | /******************************************************//** |
julientiron | 0:cba942f8172a | 181 | * @brief Returns the min speed of the specified shield |
julientiron | 0:cba942f8172a | 182 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 183 | * @retval minSpeed in pps |
julientiron | 0:cba942f8172a | 184 | **********************************************************/ |
julientiron | 0:cba942f8172a | 185 | uint16_t Easyspin::GetMinSpeed(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 186 | { |
julientiron | 0:cba942f8172a | 187 | return (shieldPrm[shieldId].minSpeed); |
julientiron | 0:cba942f8172a | 188 | } |
julientiron | 0:cba942f8172a | 189 | |
julientiron | 0:cba942f8172a | 190 | /******************************************************//** |
julientiron | 0:cba942f8172a | 191 | * @brief Returns the ABS_POSITION of the specified shield |
julientiron | 0:cba942f8172a | 192 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 193 | * @retval ABS_POSITION register value converted in a 32b signed integer |
julientiron | 0:cba942f8172a | 194 | **********************************************************/ |
julientiron | 0:cba942f8172a | 195 | int32_t Easyspin::GetPosition(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 196 | { |
julientiron | 0:cba942f8172a | 197 | return ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); |
julientiron | 0:cba942f8172a | 198 | } |
julientiron | 0:cba942f8172a | 199 | |
julientiron | 0:cba942f8172a | 200 | /******************************************************//** |
julientiron | 0:cba942f8172a | 201 | * @brief Returns the shield state |
julientiron | 0:cba942f8172a | 202 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 203 | * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE) |
julientiron | 0:cba942f8172a | 204 | **********************************************************/ |
julientiron | 0:cba942f8172a | 205 | shieldState_t Easyspin::GetShieldState(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 206 | { |
julientiron | 0:cba942f8172a | 207 | return shieldPrm[shieldId].motionState; |
julientiron | 0:cba942f8172a | 208 | } |
julientiron | 0:cba942f8172a | 209 | |
julientiron | 0:cba942f8172a | 210 | /******************************************************//** |
julientiron | 0:cba942f8172a | 211 | * @brief Requests the motor to move to the home position (ABS_POSITION = 0) |
julientiron | 0:cba942f8172a | 212 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 213 | * @retval None |
julientiron | 0:cba942f8172a | 214 | **********************************************************/ |
julientiron | 0:cba942f8172a | 215 | void Easyspin::GoHome(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 216 | { |
julientiron | 0:cba942f8172a | 217 | GoTo(shieldId, 0); |
julientiron | 0:cba942f8172a | 218 | } |
julientiron | 0:cba942f8172a | 219 | |
julientiron | 0:cba942f8172a | 220 | /******************************************************//** |
julientiron | 0:cba942f8172a | 221 | * @brief Requests the motor to move to the mark position |
julientiron | 0:cba942f8172a | 222 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 223 | * @retval None |
julientiron | 0:cba942f8172a | 224 | **********************************************************/ |
julientiron | 0:cba942f8172a | 225 | void Easyspin::GoMark(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 226 | { |
julientiron | 0:cba942f8172a | 227 | uint32_t mark; |
julientiron | 0:cba942f8172a | 228 | |
julientiron | 0:cba942f8172a | 229 | mark = ConvertPosition(CmdGetParam(shieldId,Easyspin_MARK)); |
julientiron | 0:cba942f8172a | 230 | GoTo(shieldId,mark); |
julientiron | 0:cba942f8172a | 231 | } |
julientiron | 0:cba942f8172a | 232 | |
julientiron | 0:cba942f8172a | 233 | /******************************************************//** |
julientiron | 0:cba942f8172a | 234 | * @brief Requests the motor to move to the specified position |
julientiron | 0:cba942f8172a | 235 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 236 | * @param[in] targetPosition absolute position in steps |
julientiron | 0:cba942f8172a | 237 | * @retval None |
julientiron | 0:cba942f8172a | 238 | **********************************************************/ |
julientiron | 0:cba942f8172a | 239 | void Easyspin::GoTo(uint8_t shieldId, int32_t targetPosition) |
julientiron | 0:cba942f8172a | 240 | { |
julientiron | 0:cba942f8172a | 241 | dir_t direction; |
julientiron | 0:cba942f8172a | 242 | int32_t steps; |
julientiron | 0:cba942f8172a | 243 | |
julientiron | 0:cba942f8172a | 244 | /* Eventually deactivate motor */ |
julientiron | 0:cba942f8172a | 245 | if (shieldPrm[shieldId].motionState != INACTIVE) |
julientiron | 0:cba942f8172a | 246 | { |
julientiron | 0:cba942f8172a | 247 | HardStop(shieldId); |
julientiron | 0:cba942f8172a | 248 | } |
julientiron | 0:cba942f8172a | 249 | |
julientiron | 0:cba942f8172a | 250 | /* Get current position */ |
julientiron | 0:cba942f8172a | 251 | shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); |
julientiron | 0:cba942f8172a | 252 | |
julientiron | 0:cba942f8172a | 253 | /* Compute the number of steps to perform */ |
julientiron | 0:cba942f8172a | 254 | steps = targetPosition - shieldPrm[shieldId].currentPosition; |
julientiron | 0:cba942f8172a | 255 | |
julientiron | 0:cba942f8172a | 256 | if (steps >= 0) |
julientiron | 0:cba942f8172a | 257 | { |
julientiron | 0:cba942f8172a | 258 | shieldPrm[shieldId].stepsToTake = steps; |
julientiron | 0:cba942f8172a | 259 | direction = FORWARD; |
julientiron | 0:cba942f8172a | 260 | |
julientiron | 0:cba942f8172a | 261 | } |
julientiron | 0:cba942f8172a | 262 | else |
julientiron | 0:cba942f8172a | 263 | { |
julientiron | 0:cba942f8172a | 264 | shieldPrm[shieldId].stepsToTake = -steps; |
julientiron | 0:cba942f8172a | 265 | direction = BACKWARD; |
julientiron | 0:cba942f8172a | 266 | } |
julientiron | 0:cba942f8172a | 267 | |
julientiron | 0:cba942f8172a | 268 | if (steps != 0) |
julientiron | 0:cba942f8172a | 269 | { |
julientiron | 0:cba942f8172a | 270 | |
julientiron | 0:cba942f8172a | 271 | shieldPrm[shieldId].commandExecuted = MOVE_CMD; |
julientiron | 0:cba942f8172a | 272 | |
julientiron | 0:cba942f8172a | 273 | /* Direction setup */ |
julientiron | 0:cba942f8172a | 274 | SetDirection(shieldId,direction); |
julientiron | 0:cba942f8172a | 275 | |
julientiron | 0:cba942f8172a | 276 | ComputeSpeedProfile(shieldId, shieldPrm[shieldId].stepsToTake); |
julientiron | 0:cba942f8172a | 277 | |
julientiron | 0:cba942f8172a | 278 | /* Motor activation */ |
julientiron | 0:cba942f8172a | 279 | StartMovement(shieldId); |
julientiron | 0:cba942f8172a | 280 | } |
julientiron | 0:cba942f8172a | 281 | } |
julientiron | 0:cba942f8172a | 282 | |
julientiron | 0:cba942f8172a | 283 | /******************************************************//** |
julientiron | 0:cba942f8172a | 284 | * @brief Immediatly stops the motor and disable the power bridge |
julientiron | 0:cba942f8172a | 285 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 286 | * @retval None |
julientiron | 0:cba942f8172a | 287 | **********************************************************/ |
julientiron | 0:cba942f8172a | 288 | void Easyspin::HardStop(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 289 | { |
julientiron | 0:cba942f8172a | 290 | /* Disable corresponding PWM */ |
julientiron | 0:cba942f8172a | 291 | PwmStop(shieldId); |
julientiron | 0:cba942f8172a | 292 | |
julientiron | 0:cba942f8172a | 293 | /* Disable power stage */ |
julientiron | 0:cba942f8172a | 294 | CmdDisable(shieldId); |
julientiron | 0:cba942f8172a | 295 | |
julientiron | 0:cba942f8172a | 296 | /* Set inactive state */ |
julientiron | 0:cba942f8172a | 297 | shieldPrm[shieldId].motionState = INACTIVE; |
julientiron | 0:cba942f8172a | 298 | shieldPrm[shieldId].commandExecuted = NO_CMD; |
julientiron | 0:cba942f8172a | 299 | shieldPrm[shieldId].stepsToTake = MAX_STEPS; |
julientiron | 0:cba942f8172a | 300 | |
julientiron | 0:cba942f8172a | 301 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 302 | Serial.println("Inactive\n"); |
julientiron | 0:cba942f8172a | 303 | #endif |
julientiron | 0:cba942f8172a | 304 | } |
julientiron | 0:cba942f8172a | 305 | |
julientiron | 0:cba942f8172a | 306 | /******************************************************//** |
julientiron | 0:cba942f8172a | 307 | * @brief Moves the motor of the specified number of steps |
julientiron | 0:cba942f8172a | 308 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 309 | * @param[in] direction FORWARD or BACKWARD |
julientiron | 0:cba942f8172a | 310 | * @param[in] stepCount Number of steps to perform |
julientiron | 0:cba942f8172a | 311 | * @retval None |
julientiron | 0:cba942f8172a | 312 | **********************************************************/ |
julientiron | 0:cba942f8172a | 313 | void Easyspin::Move(uint8_t shieldId, dir_t direction, uint32_t stepCount) |
julientiron | 0:cba942f8172a | 314 | { |
julientiron | 0:cba942f8172a | 315 | /* Eventually deactivate motor */ |
julientiron | 0:cba942f8172a | 316 | if (shieldPrm[shieldId].motionState != INACTIVE) |
julientiron | 0:cba942f8172a | 317 | { |
julientiron | 0:cba942f8172a | 318 | HardStop(shieldId); |
julientiron | 0:cba942f8172a | 319 | } |
julientiron | 0:cba942f8172a | 320 | |
julientiron | 0:cba942f8172a | 321 | if (stepCount != 0) |
julientiron | 0:cba942f8172a | 322 | { |
julientiron | 0:cba942f8172a | 323 | shieldPrm[shieldId].stepsToTake = stepCount; |
julientiron | 0:cba942f8172a | 324 | |
julientiron | 0:cba942f8172a | 325 | shieldPrm[shieldId].commandExecuted = MOVE_CMD; |
julientiron | 0:cba942f8172a | 326 | |
julientiron | 0:cba942f8172a | 327 | shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); |
julientiron | 0:cba942f8172a | 328 | |
julientiron | 0:cba942f8172a | 329 | /* Direction setup */ |
julientiron | 0:cba942f8172a | 330 | SetDirection(shieldId,direction); |
julientiron | 0:cba942f8172a | 331 | |
julientiron | 0:cba942f8172a | 332 | ComputeSpeedProfile(shieldId, stepCount); |
julientiron | 0:cba942f8172a | 333 | |
julientiron | 0:cba942f8172a | 334 | /* Motor activation */ |
julientiron | 0:cba942f8172a | 335 | StartMovement(shieldId); |
julientiron | 0:cba942f8172a | 336 | } |
julientiron | 0:cba942f8172a | 337 | } |
julientiron | 0:cba942f8172a | 338 | |
julientiron | 0:cba942f8172a | 339 | /******************************************************//** |
julientiron | 0:cba942f8172a | 340 | * @brief Resets all Easyspin shields |
julientiron | 0:cba942f8172a | 341 | * @param None |
julientiron | 0:cba942f8172a | 342 | * @retval None |
julientiron | 0:cba942f8172a | 343 | **********************************************************/ |
julientiron | 0:cba942f8172a | 344 | void Easyspin::ResetAllShields(void) |
julientiron | 0:cba942f8172a | 345 | { |
julientiron | 0:cba942f8172a | 346 | uint8_t loop; |
julientiron | 0:cba942f8172a | 347 | |
julientiron | 0:cba942f8172a | 348 | for (loop = 0; loop < numberOfShields; loop++) |
julientiron | 0:cba942f8172a | 349 | { |
julientiron | 0:cba942f8172a | 350 | /* Stop movement and disable power stage*/ |
julientiron | 0:cba942f8172a | 351 | HardStop(loop); |
julientiron | 0:cba942f8172a | 352 | } |
julientiron | 0:cba942f8172a | 353 | Reset(); |
julientiron | 0:cba942f8172a | 354 | WaitUs(20); // Reset pin must be forced low for at least 10us |
julientiron | 0:cba942f8172a | 355 | ReleaseReset(); |
julientiron | 0:cba942f8172a | 356 | } |
julientiron | 0:cba942f8172a | 357 | |
julientiron | 0:cba942f8172a | 358 | /******************************************************//** |
julientiron | 0:cba942f8172a | 359 | * @brief Runs the motor. It will accelerate from the min |
julientiron | 0:cba942f8172a | 360 | * speed up to the max speed by using the shield acceleration. |
julientiron | 0:cba942f8172a | 361 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 362 | * @param[in] direction FORWARD or BACKWARD |
julientiron | 0:cba942f8172a | 363 | * @retval None |
julientiron | 0:cba942f8172a | 364 | **********************************************************/ |
julientiron | 0:cba942f8172a | 365 | void Easyspin::Run(uint8_t shieldId, dir_t direction) |
julientiron | 0:cba942f8172a | 366 | { |
julientiron | 0:cba942f8172a | 367 | /* Eventually deactivate motor */ |
julientiron | 0:cba942f8172a | 368 | if (shieldPrm[shieldId].motionState != INACTIVE) |
julientiron | 0:cba942f8172a | 369 | { |
julientiron | 0:cba942f8172a | 370 | HardStop(shieldId); |
julientiron | 0:cba942f8172a | 371 | } |
julientiron | 0:cba942f8172a | 372 | |
julientiron | 0:cba942f8172a | 373 | /* Direction setup */ |
julientiron | 0:cba942f8172a | 374 | SetDirection(shieldId,direction); |
julientiron | 0:cba942f8172a | 375 | |
julientiron | 0:cba942f8172a | 376 | shieldPrm[shieldId].commandExecuted = RUN_CMD; |
julientiron | 0:cba942f8172a | 377 | |
julientiron | 0:cba942f8172a | 378 | /* Motor activation */ |
julientiron | 0:cba942f8172a | 379 | StartMovement(shieldId); |
julientiron | 0:cba942f8172a | 380 | } |
julientiron | 0:cba942f8172a | 381 | |
julientiron | 0:cba942f8172a | 382 | /******************************************************//** |
julientiron | 0:cba942f8172a | 383 | * @brief Changes the acceleration of the specified shield |
julientiron | 0:cba942f8172a | 384 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 385 | * @param[in] newAcc New acceleration to apply in pps^2 |
julientiron | 0:cba942f8172a | 386 | * @retval true if the command is successfully executed, else false |
julientiron | 0:cba942f8172a | 387 | * @note The command is not performed is the shield is executing |
julientiron | 0:cba942f8172a | 388 | * a MOVE or GOTO command (but it can be used during a RUN command) |
julientiron | 0:cba942f8172a | 389 | **********************************************************/ |
julientiron | 0:cba942f8172a | 390 | bool Easyspin::SetAcceleration(uint8_t shieldId,uint16_t newAcc) |
julientiron | 0:cba942f8172a | 391 | { |
julientiron | 0:cba942f8172a | 392 | bool cmdExecuted = false; |
julientiron | 0:cba942f8172a | 393 | if ((newAcc != 0)&& |
julientiron | 0:cba942f8172a | 394 | ((shieldPrm[shieldId].motionState == INACTIVE)|| |
julientiron | 0:cba942f8172a | 395 | (shieldPrm[shieldId].commandExecuted == RUN_CMD))) |
julientiron | 0:cba942f8172a | 396 | { |
julientiron | 0:cba942f8172a | 397 | shieldPrm[shieldId].acceleration = newAcc; |
julientiron | 0:cba942f8172a | 398 | cmdExecuted = true; |
julientiron | 0:cba942f8172a | 399 | } |
julientiron | 0:cba942f8172a | 400 | return cmdExecuted; |
julientiron | 0:cba942f8172a | 401 | } |
julientiron | 0:cba942f8172a | 402 | |
julientiron | 0:cba942f8172a | 403 | /******************************************************//** |
julientiron | 0:cba942f8172a | 404 | * @brief Changes the deceleration of the specified shield |
julientiron | 0:cba942f8172a | 405 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 406 | * @param[in] newDec New deceleration to apply in pps^2 |
julientiron | 0:cba942f8172a | 407 | * @retval true if the command is successfully executed, else false |
julientiron | 0:cba942f8172a | 408 | * @note The command is not performed is the shield is executing |
julientiron | 0:cba942f8172a | 409 | * a MOVE or GOTO command (but it can be used during a RUN command) |
julientiron | 0:cba942f8172a | 410 | **********************************************************/ |
julientiron | 0:cba942f8172a | 411 | bool Easyspin::SetDeceleration(uint8_t shieldId, uint16_t newDec) |
julientiron | 0:cba942f8172a | 412 | { |
julientiron | 0:cba942f8172a | 413 | bool cmdExecuted = false; |
julientiron | 0:cba942f8172a | 414 | if ((newDec != 0)&& |
julientiron | 0:cba942f8172a | 415 | ((shieldPrm[shieldId].motionState == INACTIVE)|| |
julientiron | 0:cba942f8172a | 416 | (shieldPrm[shieldId].commandExecuted == RUN_CMD))) |
julientiron | 0:cba942f8172a | 417 | { |
julientiron | 0:cba942f8172a | 418 | shieldPrm[shieldId].deceleration = newDec; |
julientiron | 0:cba942f8172a | 419 | cmdExecuted = true; |
julientiron | 0:cba942f8172a | 420 | } |
julientiron | 0:cba942f8172a | 421 | return cmdExecuted; |
julientiron | 0:cba942f8172a | 422 | } |
julientiron | 0:cba942f8172a | 423 | |
julientiron | 0:cba942f8172a | 424 | /******************************************************//** |
julientiron | 0:cba942f8172a | 425 | * @brief Set current position to be the Home position (ABS pos set to 0) |
julientiron | 0:cba942f8172a | 426 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 427 | * @retval None |
julientiron | 0:cba942f8172a | 428 | **********************************************************/ |
julientiron | 0:cba942f8172a | 429 | void Easyspin::SetHome(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 430 | { |
julientiron | 0:cba942f8172a | 431 | CmdSetParam(shieldId, Easyspin_ABS_POS, 0); |
julientiron | 0:cba942f8172a | 432 | } |
julientiron | 0:cba942f8172a | 433 | |
julientiron | 0:cba942f8172a | 434 | /******************************************************//** |
julientiron | 0:cba942f8172a | 435 | * @brief Sets current position to be the Mark position |
julientiron | 0:cba942f8172a | 436 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 437 | * @retval None |
julientiron | 0:cba942f8172a | 438 | **********************************************************/ |
julientiron | 0:cba942f8172a | 439 | void Easyspin::SetMark(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 440 | { |
julientiron | 0:cba942f8172a | 441 | uint32_t mark = CmdGetParam(shieldId,Easyspin_ABS_POS); |
julientiron | 0:cba942f8172a | 442 | CmdSetParam(shieldId,Easyspin_MARK, mark); |
julientiron | 0:cba942f8172a | 443 | } |
julientiron | 0:cba942f8172a | 444 | |
julientiron | 0:cba942f8172a | 445 | /******************************************************//** |
julientiron | 0:cba942f8172a | 446 | * @brief Changes the max speed of the specified shield |
julientiron | 0:cba942f8172a | 447 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 448 | * @param[in] newMaxSpeed New max speed to apply in pps |
julientiron | 0:cba942f8172a | 449 | * @retval true if the command is successfully executed, else false |
julientiron | 0:cba942f8172a | 450 | * @note The command is not performed is the shield is executing |
julientiron | 0:cba942f8172a | 451 | * a MOVE or GOTO command (but it can be used during a RUN command). |
julientiron | 0:cba942f8172a | 452 | **********************************************************/ |
julientiron | 0:cba942f8172a | 453 | bool Easyspin::SetMaxSpeed(uint8_t shieldId, uint16_t newMaxSpeed) |
julientiron | 0:cba942f8172a | 454 | { |
julientiron | 0:cba942f8172a | 455 | bool cmdExecuted = false; |
julientiron | 0:cba942f8172a | 456 | if ((newMaxSpeed > Easyspin_MIN_PWM_FREQ)&& |
julientiron | 0:cba942f8172a | 457 | (newMaxSpeed <= Easyspin_MAX_PWM_FREQ) && |
julientiron | 0:cba942f8172a | 458 | (shieldPrm[shieldId].minSpeed <= newMaxSpeed) && |
julientiron | 0:cba942f8172a | 459 | ((shieldPrm[shieldId].motionState == INACTIVE)|| |
julientiron | 0:cba942f8172a | 460 | (shieldPrm[shieldId].commandExecuted == RUN_CMD))) |
julientiron | 0:cba942f8172a | 461 | { |
julientiron | 0:cba942f8172a | 462 | shieldPrm[shieldId].maxSpeed = newMaxSpeed; |
julientiron | 0:cba942f8172a | 463 | cmdExecuted = true; |
julientiron | 0:cba942f8172a | 464 | } |
julientiron | 0:cba942f8172a | 465 | return cmdExecuted; |
julientiron | 0:cba942f8172a | 466 | } |
julientiron | 0:cba942f8172a | 467 | |
julientiron | 0:cba942f8172a | 468 | /******************************************************//** |
julientiron | 0:cba942f8172a | 469 | * @brief Changes the min speed of the specified shield |
julientiron | 0:cba942f8172a | 470 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 471 | * @param[in] newMinSpeed New min speed to apply in pps |
julientiron | 0:cba942f8172a | 472 | * @retval true if the command is successfully executed, else false |
julientiron | 0:cba942f8172a | 473 | * @note The command is not performed is the shield is executing |
julientiron | 0:cba942f8172a | 474 | * a MOVE or GOTO command (but it can be used during a RUN command). |
julientiron | 0:cba942f8172a | 475 | **********************************************************/ |
julientiron | 0:cba942f8172a | 476 | bool Easyspin::SetMinSpeed(uint8_t shieldId, uint16_t newMinSpeed) |
julientiron | 0:cba942f8172a | 477 | { |
julientiron | 0:cba942f8172a | 478 | bool cmdExecuted = false; |
julientiron | 0:cba942f8172a | 479 | if ((newMinSpeed >= Easyspin_MIN_PWM_FREQ)&& |
julientiron | 0:cba942f8172a | 480 | (newMinSpeed < Easyspin_MAX_PWM_FREQ) && |
julientiron | 0:cba942f8172a | 481 | (newMinSpeed <= shieldPrm[shieldId].maxSpeed) && |
julientiron | 0:cba942f8172a | 482 | ((shieldPrm[shieldId].motionState == INACTIVE)|| |
julientiron | 0:cba942f8172a | 483 | (shieldPrm[shieldId].commandExecuted == RUN_CMD))) |
julientiron | 0:cba942f8172a | 484 | { |
julientiron | 0:cba942f8172a | 485 | shieldPrm[shieldId].minSpeed = newMinSpeed; |
julientiron | 0:cba942f8172a | 486 | cmdExecuted = true; |
julientiron | 0:cba942f8172a | 487 | } |
julientiron | 0:cba942f8172a | 488 | return cmdExecuted; |
julientiron | 0:cba942f8172a | 489 | } |
julientiron | 0:cba942f8172a | 490 | |
julientiron | 0:cba942f8172a | 491 | /******************************************************//** |
julientiron | 0:cba942f8172a | 492 | * @brief Stops the motor by using the shield deceleration |
julientiron | 0:cba942f8172a | 493 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 494 | * @retval true if the command is successfully executed, else false |
julientiron | 0:cba942f8172a | 495 | * @note The command is not performed is the shield is in INACTIVE state. |
julientiron | 0:cba942f8172a | 496 | **********************************************************/ |
julientiron | 0:cba942f8172a | 497 | bool Easyspin::SoftStop(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 498 | { |
julientiron | 0:cba942f8172a | 499 | bool cmdExecuted = false; |
julientiron | 0:cba942f8172a | 500 | if (shieldPrm[shieldId].motionState != INACTIVE) |
julientiron | 0:cba942f8172a | 501 | { |
julientiron | 0:cba942f8172a | 502 | shieldPrm[shieldId].commandExecuted = SOFT_STOP_CMD; |
julientiron | 0:cba942f8172a | 503 | cmdExecuted = true; |
julientiron | 0:cba942f8172a | 504 | } |
julientiron | 0:cba942f8172a | 505 | return (cmdExecuted); |
julientiron | 0:cba942f8172a | 506 | } |
julientiron | 0:cba942f8172a | 507 | |
julientiron | 0:cba942f8172a | 508 | /******************************************************//** |
julientiron | 0:cba942f8172a | 509 | * @brief Locks until the shield state becomes Inactive |
julientiron | 0:cba942f8172a | 510 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 511 | * @retval None |
julientiron | 0:cba942f8172a | 512 | **********************************************************/ |
julientiron | 0:cba942f8172a | 513 | void Easyspin::WaitWhileActive(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 514 | { |
julientiron | 0:cba942f8172a | 515 | /* Wait while motor is running */ |
julientiron | 0:cba942f8172a | 516 | while (GetShieldState(shieldId) != INACTIVE); |
julientiron | 0:cba942f8172a | 517 | } |
julientiron | 0:cba942f8172a | 518 | |
julientiron | 0:cba942f8172a | 519 | /******************************************************//** |
julientiron | 0:cba942f8172a | 520 | * @brief Issue the Disable command to the Easyspin of the specified shield |
julientiron | 0:cba942f8172a | 521 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 522 | * @retval None |
julientiron | 0:cba942f8172a | 523 | **********************************************************/ |
julientiron | 0:cba942f8172a | 524 | void Easyspin::CmdDisable(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 525 | { |
julientiron | 0:cba942f8172a | 526 | SendCommand(shieldId, Easyspin_DISABLE); |
julientiron | 0:cba942f8172a | 527 | } |
julientiron | 0:cba942f8172a | 528 | |
julientiron | 0:cba942f8172a | 529 | /******************************************************//** |
julientiron | 0:cba942f8172a | 530 | * @brief Issues the Enable command to the Easyspin of the specified shield |
julientiron | 0:cba942f8172a | 531 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 532 | * @retval None |
julientiron | 0:cba942f8172a | 533 | **********************************************************/ |
julientiron | 0:cba942f8172a | 534 | void Easyspin::CmdEnable(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 535 | { |
julientiron | 0:cba942f8172a | 536 | SendCommand(shieldId, Easyspin_ENABLE); |
julientiron | 0:cba942f8172a | 537 | } |
julientiron | 0:cba942f8172a | 538 | |
julientiron | 0:cba942f8172a | 539 | /******************************************************//** |
julientiron | 0:cba942f8172a | 540 | * @brief Issues the GetParam command to the Easyspin of the specified shield |
julientiron | 0:cba942f8172a | 541 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 542 | * @param[in] param Register adress (Easyspin_ABS_POS, Easyspin_MARK,...) |
julientiron | 0:cba942f8172a | 543 | * @retval Register value |
julientiron | 0:cba942f8172a | 544 | **********************************************************/ |
julientiron | 0:cba942f8172a | 545 | uint32_t Easyspin::CmdGetParam(uint8_t shieldId, Easyspin_Registers_t param) |
julientiron | 0:cba942f8172a | 546 | { |
julientiron | 0:cba942f8172a | 547 | uint32_t i; |
julientiron | 0:cba942f8172a | 548 | uint32_t spiRxData; |
julientiron | 0:cba942f8172a | 549 | uint8_t maxArgumentNbBytes = 0; |
julientiron | 0:cba942f8172a | 550 | uint8_t spiIndex = numberOfShields - shieldId - 1; |
julientiron | 0:cba942f8172a | 551 | bool itDisable = false; |
julientiron | 0:cba942f8172a | 552 | |
julientiron | 0:cba942f8172a | 553 | do |
julientiron | 0:cba942f8172a | 554 | { |
julientiron | 0:cba942f8172a | 555 | spiPreemtionByIsr = false; |
julientiron | 0:cba942f8172a | 556 | if (itDisable) |
julientiron | 0:cba942f8172a | 557 | { |
julientiron | 0:cba942f8172a | 558 | /* re-enable interrupts if disable in previous iteration */ |
julientiron | 0:cba942f8172a | 559 | flag.enable_irq(); |
julientiron | 0:cba942f8172a | 560 | itDisable = false; |
julientiron | 0:cba942f8172a | 561 | } |
julientiron | 0:cba942f8172a | 562 | |
julientiron | 0:cba942f8172a | 563 | for (i = 0; i < numberOfShields; i++) |
julientiron | 0:cba942f8172a | 564 | { |
julientiron | 0:cba942f8172a | 565 | spiTxBursts[0][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 566 | spiTxBursts[1][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 567 | spiTxBursts[2][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 568 | spiTxBursts[3][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 569 | spiRxBursts[1][i] = 0; |
julientiron | 0:cba942f8172a | 570 | spiRxBursts[2][i] = 0; |
julientiron | 0:cba942f8172a | 571 | spiRxBursts[3][i] = 0; |
julientiron | 0:cba942f8172a | 572 | } |
julientiron | 0:cba942f8172a | 573 | switch (param) |
julientiron | 0:cba942f8172a | 574 | { |
julientiron | 0:cba942f8172a | 575 | case Easyspin_ABS_POS: ; |
julientiron | 0:cba942f8172a | 576 | case Easyspin_MARK: |
julientiron | 0:cba942f8172a | 577 | spiTxBursts[0][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param); |
julientiron | 0:cba942f8172a | 578 | maxArgumentNbBytes = 3; |
julientiron | 0:cba942f8172a | 579 | break; |
julientiron | 0:cba942f8172a | 580 | case Easyspin_EL_POS: ; |
julientiron | 0:cba942f8172a | 581 | case Easyspin_CONFIG: ; |
julientiron | 0:cba942f8172a | 582 | case Easyspin_STATUS: |
julientiron | 0:cba942f8172a | 583 | spiTxBursts[1][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param); |
julientiron | 0:cba942f8172a | 584 | maxArgumentNbBytes = 2; |
julientiron | 0:cba942f8172a | 585 | break; |
julientiron | 0:cba942f8172a | 586 | default: |
julientiron | 0:cba942f8172a | 587 | spiTxBursts[2][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param); |
julientiron | 0:cba942f8172a | 588 | maxArgumentNbBytes = 1; |
julientiron | 0:cba942f8172a | 589 | } |
julientiron | 0:cba942f8172a | 590 | |
julientiron | 0:cba942f8172a | 591 | /* Disable interruption before checking */ |
julientiron | 0:cba942f8172a | 592 | /* pre-emption by ISR and SPI transfers*/ |
julientiron | 0:cba942f8172a | 593 | flag.disable_irq(); |
julientiron | 0:cba942f8172a | 594 | itDisable = true; |
julientiron | 0:cba942f8172a | 595 | } while (spiPreemtionByIsr); // check pre-emption by ISR |
julientiron | 0:cba942f8172a | 596 | |
julientiron | 0:cba942f8172a | 597 | for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; |
julientiron | 0:cba942f8172a | 598 | i < Easyspin_CMD_ARG_MAX_NB_BYTES; |
julientiron | 0:cba942f8172a | 599 | i++) |
julientiron | 0:cba942f8172a | 600 | { |
julientiron | 0:cba942f8172a | 601 | WriteBytes(&spiTxBursts[i][0], |
julientiron | 0:cba942f8172a | 602 | &spiRxBursts[i][0]); |
julientiron | 0:cba942f8172a | 603 | } |
julientiron | 0:cba942f8172a | 604 | |
julientiron | 0:cba942f8172a | 605 | spiRxData = ((uint32_t)spiRxBursts[1][spiIndex] << 16)| |
julientiron | 0:cba942f8172a | 606 | (spiRxBursts[2][spiIndex] << 8) | |
julientiron | 0:cba942f8172a | 607 | (spiRxBursts[3][spiIndex]); |
julientiron | 0:cba942f8172a | 608 | |
julientiron | 0:cba942f8172a | 609 | /* re-enable interrupts after SPI transfers*/ |
julientiron | 0:cba942f8172a | 610 | flag.enable_irq(); |
julientiron | 0:cba942f8172a | 611 | |
julientiron | 0:cba942f8172a | 612 | return (spiRxData); |
julientiron | 0:cba942f8172a | 613 | } |
julientiron | 0:cba942f8172a | 614 | |
julientiron | 0:cba942f8172a | 615 | /******************************************************//** |
julientiron | 0:cba942f8172a | 616 | * @brief Issues the GetStatus command to the Easyspin of the specified shield |
julientiron | 0:cba942f8172a | 617 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 618 | * @retval Status Register value |
julientiron | 0:cba942f8172a | 619 | * @note Once the GetStatus command is performed, the flags of the status register |
julientiron | 0:cba942f8172a | 620 | * are reset. This is not the case when the status register is read with the |
julientiron | 0:cba942f8172a | 621 | * GetParam command (via the functions ReadStatusRegister or CmdGetParam). |
julientiron | 0:cba942f8172a | 622 | **********************************************************/ |
julientiron | 0:cba942f8172a | 623 | uint16_t Easyspin::CmdGetStatus(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 624 | { |
julientiron | 0:cba942f8172a | 625 | uint32_t i; |
julientiron | 0:cba942f8172a | 626 | uint16_t status; |
julientiron | 0:cba942f8172a | 627 | uint8_t spiIndex = numberOfShields - shieldId - 1; |
julientiron | 0:cba942f8172a | 628 | bool itDisable = false; |
julientiron | 0:cba942f8172a | 629 | |
julientiron | 0:cba942f8172a | 630 | do |
julientiron | 0:cba942f8172a | 631 | { |
julientiron | 0:cba942f8172a | 632 | spiPreemtionByIsr = false; |
julientiron | 0:cba942f8172a | 633 | if (itDisable) |
julientiron | 0:cba942f8172a | 634 | { |
julientiron | 0:cba942f8172a | 635 | /* re-enable interrupts if disable in previous iteration */ |
julientiron | 0:cba942f8172a | 636 | flag.enable_irq(); |
julientiron | 0:cba942f8172a | 637 | itDisable = false; |
julientiron | 0:cba942f8172a | 638 | } |
julientiron | 0:cba942f8172a | 639 | |
julientiron | 0:cba942f8172a | 640 | for (i = 0; i < numberOfShields; i++) |
julientiron | 0:cba942f8172a | 641 | { |
julientiron | 0:cba942f8172a | 642 | spiTxBursts[0][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 643 | spiTxBursts[1][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 644 | spiTxBursts[2][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 645 | spiRxBursts[1][i] = 0; |
julientiron | 0:cba942f8172a | 646 | spiRxBursts[2][i] = 0; |
julientiron | 0:cba942f8172a | 647 | } |
julientiron | 0:cba942f8172a | 648 | spiTxBursts[0][spiIndex] = Easyspin_GET_STATUS; |
julientiron | 0:cba942f8172a | 649 | |
julientiron | 0:cba942f8172a | 650 | /* Disable interruption before checking */ |
julientiron | 0:cba942f8172a | 651 | /* pre-emption by ISR and SPI transfers*/ |
julientiron | 0:cba942f8172a | 652 | flag.disable_irq(); |
julientiron | 0:cba942f8172a | 653 | itDisable = true; |
julientiron | 0:cba942f8172a | 654 | } while (spiPreemtionByIsr); // check pre-emption by ISR |
julientiron | 0:cba942f8172a | 655 | |
julientiron | 0:cba942f8172a | 656 | for (i = 0; i < Easyspin_CMD_ARG_NB_BYTES_GET_STATUS + Easyspin_RSP_NB_BYTES_GET_STATUS; i++) |
julientiron | 0:cba942f8172a | 657 | { |
julientiron | 0:cba942f8172a | 658 | WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]); |
julientiron | 0:cba942f8172a | 659 | } |
julientiron | 0:cba942f8172a | 660 | status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]); |
julientiron | 0:cba942f8172a | 661 | |
julientiron | 0:cba942f8172a | 662 | /* re-enable interrupts after SPI transfers*/ |
julientiron | 0:cba942f8172a | 663 | flag.enable_irq(); |
julientiron | 0:cba942f8172a | 664 | |
julientiron | 0:cba942f8172a | 665 | return (status); |
julientiron | 0:cba942f8172a | 666 | } |
julientiron | 0:cba942f8172a | 667 | |
julientiron | 0:cba942f8172a | 668 | /******************************************************//** |
julientiron | 0:cba942f8172a | 669 | * @brief Issues the Nop command to the Easyspin of the specified shield |
julientiron | 0:cba942f8172a | 670 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 671 | * @retval None |
julientiron | 0:cba942f8172a | 672 | **********************************************************/ |
julientiron | 0:cba942f8172a | 673 | void Easyspin::CmdNop(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 674 | { |
julientiron | 0:cba942f8172a | 675 | SendCommand(shieldId, Easyspin_NOP); |
julientiron | 0:cba942f8172a | 676 | } |
julientiron | 0:cba942f8172a | 677 | |
julientiron | 0:cba942f8172a | 678 | /******************************************************//** |
julientiron | 0:cba942f8172a | 679 | * @brief Issues the SetParam command to the Easyspin of the specified shield |
julientiron | 0:cba942f8172a | 680 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 681 | * @param[in] param Register adress (Easyspin_ABS_POS, Easyspin_MARK,...) |
julientiron | 0:cba942f8172a | 682 | * @param[in] value Value to set in the register |
julientiron | 0:cba942f8172a | 683 | * @retval None |
julientiron | 0:cba942f8172a | 684 | **********************************************************/ |
julientiron | 0:cba942f8172a | 685 | void Easyspin::CmdSetParam(uint8_t shieldId, |
julientiron | 0:cba942f8172a | 686 | Easyspin_Registers_t param, |
julientiron | 0:cba942f8172a | 687 | uint32_t value) |
julientiron | 0:cba942f8172a | 688 | { |
julientiron | 0:cba942f8172a | 689 | uint32_t i; |
julientiron | 0:cba942f8172a | 690 | uint8_t maxArgumentNbBytes = 0; |
julientiron | 0:cba942f8172a | 691 | uint8_t spiIndex = numberOfShields - shieldId - 1; |
julientiron | 0:cba942f8172a | 692 | bool itDisable = false; |
julientiron | 0:cba942f8172a | 693 | do |
julientiron | 0:cba942f8172a | 694 | { |
julientiron | 0:cba942f8172a | 695 | spiPreemtionByIsr = false; |
julientiron | 0:cba942f8172a | 696 | if (itDisable) |
julientiron | 0:cba942f8172a | 697 | { |
julientiron | 0:cba942f8172a | 698 | /* re-enable interrupts if disable in previous iteration */ |
julientiron | 0:cba942f8172a | 699 | flag.enable_irq();//interrupts(); |
julientiron | 0:cba942f8172a | 700 | itDisable = false; |
julientiron | 0:cba942f8172a | 701 | } |
julientiron | 0:cba942f8172a | 702 | for (i = 0; i < numberOfShields; i++) |
julientiron | 0:cba942f8172a | 703 | { |
julientiron | 0:cba942f8172a | 704 | spiTxBursts[0][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 705 | spiTxBursts[1][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 706 | spiTxBursts[2][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 707 | spiTxBursts[3][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 708 | } |
julientiron | 0:cba942f8172a | 709 | switch (param) |
julientiron | 0:cba942f8172a | 710 | { |
julientiron | 0:cba942f8172a | 711 | case Easyspin_ABS_POS: ; |
julientiron | 0:cba942f8172a | 712 | case Easyspin_MARK: |
julientiron | 0:cba942f8172a | 713 | spiTxBursts[0][spiIndex] = param; |
julientiron | 0:cba942f8172a | 714 | spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16); |
julientiron | 0:cba942f8172a | 715 | spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); |
julientiron | 0:cba942f8172a | 716 | maxArgumentNbBytes = 3; |
julientiron | 0:cba942f8172a | 717 | break; |
julientiron | 0:cba942f8172a | 718 | case Easyspin_EL_POS: ; |
julientiron | 0:cba942f8172a | 719 | case Easyspin_CONFIG: |
julientiron | 0:cba942f8172a | 720 | spiTxBursts[1][spiIndex] = param; |
julientiron | 0:cba942f8172a | 721 | spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8); |
julientiron | 0:cba942f8172a | 722 | maxArgumentNbBytes = 2; |
julientiron | 0:cba942f8172a | 723 | break; |
julientiron | 0:cba942f8172a | 724 | default: |
julientiron | 0:cba942f8172a | 725 | spiTxBursts[2][spiIndex] = param; |
julientiron | 0:cba942f8172a | 726 | maxArgumentNbBytes = 1; |
julientiron | 0:cba942f8172a | 727 | } |
julientiron | 0:cba942f8172a | 728 | spiTxBursts[3][spiIndex] = (uint8_t)(value); |
julientiron | 0:cba942f8172a | 729 | |
julientiron | 0:cba942f8172a | 730 | /* Disable interruption before checking */ |
julientiron | 0:cba942f8172a | 731 | /* pre-emption by ISR and SPI transfers*/ |
julientiron | 0:cba942f8172a | 732 | flag.disable_irq(); |
julientiron | 0:cba942f8172a | 733 | itDisable = true; |
julientiron | 0:cba942f8172a | 734 | } while (spiPreemtionByIsr); // check pre-emption by ISR |
julientiron | 0:cba942f8172a | 735 | |
julientiron | 0:cba942f8172a | 736 | /* SPI transfer */ |
julientiron | 0:cba942f8172a | 737 | for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes; |
julientiron | 0:cba942f8172a | 738 | i < Easyspin_CMD_ARG_MAX_NB_BYTES; |
julientiron | 0:cba942f8172a | 739 | i++) |
julientiron | 0:cba942f8172a | 740 | { |
julientiron | 0:cba942f8172a | 741 | WriteBytes(&spiTxBursts[i][0],&spiRxBursts[i][0]); |
julientiron | 0:cba942f8172a | 742 | } |
julientiron | 0:cba942f8172a | 743 | /* re-enable interrupts after SPI transfers*/ |
julientiron | 0:cba942f8172a | 744 | flag.enable_irq(); |
julientiron | 0:cba942f8172a | 745 | } |
julientiron | 0:cba942f8172a | 746 | |
julientiron | 0:cba942f8172a | 747 | /******************************************************//** |
julientiron | 0:cba942f8172a | 748 | * @brief Reads the Status Register value |
julientiron | 0:cba942f8172a | 749 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 750 | * @retval Status register valued |
julientiron | 0:cba942f8172a | 751 | * @note The status register flags are not cleared |
julientiron | 0:cba942f8172a | 752 | * at the difference with CmdGetStatus() |
julientiron | 0:cba942f8172a | 753 | **********************************************************/ |
julientiron | 0:cba942f8172a | 754 | uint16_t Easyspin::ReadStatusRegister(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 755 | { |
julientiron | 0:cba942f8172a | 756 | return (CmdGetParam(shieldId,Easyspin_STATUS)); |
julientiron | 0:cba942f8172a | 757 | } |
julientiron | 0:cba942f8172a | 758 | |
julientiron | 0:cba942f8172a | 759 | /******************************************************//** |
julientiron | 0:cba942f8172a | 760 | * @brief Releases the Easyspin reset (pin set to High) of all shields |
julientiron | 0:cba942f8172a | 761 | * @param None |
julientiron | 0:cba942f8172a | 762 | * @retval None |
julientiron | 0:cba942f8172a | 763 | **********************************************************/ |
julientiron | 0:cba942f8172a | 764 | void Easyspin::ReleaseReset(void) |
julientiron | 0:cba942f8172a | 765 | { |
julientiron | 0:cba942f8172a | 766 | reset = 1; |
julientiron | 0:cba942f8172a | 767 | } |
julientiron | 0:cba942f8172a | 768 | |
julientiron | 0:cba942f8172a | 769 | /******************************************************//** |
julientiron | 0:cba942f8172a | 770 | * @brief Resets the Easyspin (reset pin set to low) of all shields |
julientiron | 0:cba942f8172a | 771 | * @param None |
julientiron | 0:cba942f8172a | 772 | * @retval None |
julientiron | 0:cba942f8172a | 773 | **********************************************************/ |
julientiron | 0:cba942f8172a | 774 | void Easyspin::Reset(void) |
julientiron | 0:cba942f8172a | 775 | { |
julientiron | 0:cba942f8172a | 776 | reset = 0; |
julientiron | 0:cba942f8172a | 777 | } |
julientiron | 0:cba942f8172a | 778 | |
julientiron | 0:cba942f8172a | 779 | /******************************************************//** |
julientiron | 0:cba942f8172a | 780 | * @brief Set the stepping mode |
julientiron | 0:cba942f8172a | 781 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 782 | * @param[in] stepMod from full step to 1/16 microstep as specified in enum Easyspin_STEP_SEL_t |
julientiron | 0:cba942f8172a | 783 | * @retval None |
julientiron | 0:cba942f8172a | 784 | **********************************************************/ |
julientiron | 0:cba942f8172a | 785 | void Easyspin::SelectStepMode(uint8_t shieldId, Easyspin_STEP_SEL_t stepMod) |
julientiron | 0:cba942f8172a | 786 | { |
julientiron | 0:cba942f8172a | 787 | uint8_t stepModeRegister; |
julientiron | 0:cba942f8172a | 788 | |
julientiron | 0:cba942f8172a | 789 | /* Eventually deactivate motor */ |
julientiron | 0:cba942f8172a | 790 | if (shieldPrm[shieldId].motionState != INACTIVE) |
julientiron | 0:cba942f8172a | 791 | { |
julientiron | 0:cba942f8172a | 792 | HardStop(shieldId); |
julientiron | 0:cba942f8172a | 793 | } |
julientiron | 0:cba942f8172a | 794 | |
julientiron | 0:cba942f8172a | 795 | /* Read Step mode register and clear STEP_SEL field */ |
julientiron | 0:cba942f8172a | 796 | stepModeRegister = (uint8_t)(0xF8 & CmdGetParam(shieldId,Easyspin_STEP_MODE)) ; |
julientiron | 0:cba942f8172a | 797 | |
julientiron | 0:cba942f8172a | 798 | /* Apply new step mode */ |
julientiron | 0:cba942f8172a | 799 | CmdSetParam(shieldId, Easyspin_STEP_MODE, stepModeRegister | (uint8_t)stepMod); |
julientiron | 0:cba942f8172a | 800 | |
julientiron | 0:cba942f8172a | 801 | /* Reset abs pos register */ |
julientiron | 0:cba942f8172a | 802 | SetHome(shieldId); |
julientiron | 0:cba942f8172a | 803 | } |
julientiron | 0:cba942f8172a | 804 | |
julientiron | 0:cba942f8172a | 805 | /******************************************************//** |
julientiron | 0:cba942f8172a | 806 | * @brief Specifies the direction |
julientiron | 0:cba942f8172a | 807 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 808 | * @param[in] dir FORWARD or BACKWARD |
julientiron | 0:cba942f8172a | 809 | * @note The direction change is only applied if the shield |
julientiron | 0:cba942f8172a | 810 | * is in INACTIVE state |
julientiron | 0:cba942f8172a | 811 | * @retval None |
julientiron | 0:cba942f8172a | 812 | **********************************************************/ |
julientiron | 0:cba942f8172a | 813 | void Easyspin::SetDirection(uint8_t shieldId, dir_t dir) |
julientiron | 0:cba942f8172a | 814 | { |
julientiron | 0:cba942f8172a | 815 | if (shieldPrm[shieldId].motionState == INACTIVE) |
julientiron | 0:cba942f8172a | 816 | { |
julientiron | 0:cba942f8172a | 817 | shieldPrm[shieldId].direction = dir; |
julientiron | 0:cba942f8172a | 818 | |
julientiron | 0:cba942f8172a | 819 | switch (shieldId) |
julientiron | 0:cba942f8172a | 820 | { |
julientiron | 0:cba942f8172a | 821 | case 2: |
julientiron | 0:cba942f8172a | 822 | dir3 = dir; |
julientiron | 0:cba942f8172a | 823 | break; |
julientiron | 0:cba942f8172a | 824 | case 1: |
julientiron | 0:cba942f8172a | 825 | dir2 = dir; |
julientiron | 0:cba942f8172a | 826 | break; |
julientiron | 0:cba942f8172a | 827 | case 0: |
julientiron | 0:cba942f8172a | 828 | dir1 = dir; |
julientiron | 0:cba942f8172a | 829 | break; |
julientiron | 0:cba942f8172a | 830 | default: |
julientiron | 0:cba942f8172a | 831 | ; |
julientiron | 0:cba942f8172a | 832 | } |
julientiron | 0:cba942f8172a | 833 | } |
julientiron | 0:cba942f8172a | 834 | } |
julientiron | 0:cba942f8172a | 835 | |
julientiron | 0:cba942f8172a | 836 | /******************************************************//** |
julientiron | 0:cba942f8172a | 837 | * @brief Waits for the specify delay in milliseconds |
julientiron | 0:cba942f8172a | 838 | * @param[in] msDelay delay in milliseconds |
julientiron | 0:cba942f8172a | 839 | * @retval None |
julientiron | 0:cba942f8172a | 840 | * @note Should only be used for 3 shields configuration. |
julientiron | 0:cba942f8172a | 841 | * Else, prefer the standard Arduino function delay(). |
julientiron | 0:cba942f8172a | 842 | **********************************************************/ |
julientiron | 0:cba942f8172a | 843 | void Easyspin::WaitMs(uint16_t msDelay) |
julientiron | 0:cba942f8172a | 844 | { |
julientiron | 0:cba942f8172a | 845 | uint16_t i; |
julientiron | 0:cba942f8172a | 846 | for (i = 0; i < msDelay ; i++) |
julientiron | 0:cba942f8172a | 847 | { |
julientiron | 0:cba942f8172a | 848 | WaitUs(1000); |
julientiron | 0:cba942f8172a | 849 | } |
julientiron | 0:cba942f8172a | 850 | } |
julientiron | 0:cba942f8172a | 851 | |
julientiron | 0:cba942f8172a | 852 | /******************************************************//** |
julientiron | 0:cba942f8172a | 853 | * @brief Waits for the specify delay in microseconds |
julientiron | 0:cba942f8172a | 854 | * @param[in] usDelay delay in microseconds |
julientiron | 0:cba942f8172a | 855 | * @retval None |
julientiron | 0:cba942f8172a | 856 | * @note Should be only used for 3 shields configuration. |
julientiron | 0:cba942f8172a | 857 | * Else, prefer the standard Arduino function delayMicroseconds(). |
julientiron | 0:cba942f8172a | 858 | * Besides, this function is a copy of delayMicroseconds inside |
julientiron | 0:cba942f8172a | 859 | * the Easyspin library to avoid dependencies conflicts |
julientiron | 0:cba942f8172a | 860 | * (a redefinition of ISR(TIMER0_OVF_vect)). |
julientiron | 0:cba942f8172a | 861 | **********************************************************/ |
julientiron | 0:cba942f8172a | 862 | void Easyspin::WaitUs(uint16_t usDelay) |
julientiron | 0:cba942f8172a | 863 | { |
julientiron | 0:cba942f8172a | 864 | // calling avrlib's delay_us() function with low values (e.g. 1 or |
julientiron | 0:cba942f8172a | 865 | // 2 microseconds) gives delays longer than desired. |
julientiron | 0:cba942f8172a | 866 | //delay_us(us); |
julientiron | 0:cba942f8172a | 867 | #if F_CPU >= 20000000L |
julientiron | 0:cba942f8172a | 868 | // for the 20 MHz clock on rare Arduino boards |
julientiron | 0:cba942f8172a | 869 | |
julientiron | 0:cba942f8172a | 870 | // for a one-microsecond delay, simply wait 2 cycle and return. The overhead |
julientiron | 0:cba942f8172a | 871 | // of the function call yields a delay of exactly a one microsecond. |
julientiron | 0:cba942f8172a | 872 | __asm__ __volatile__ ( |
julientiron | 0:cba942f8172a | 873 | "nop" "\n\t" |
julientiron | 0:cba942f8172a | 874 | "nop"); //just waiting 2 cycle |
julientiron | 0:cba942f8172a | 875 | if (--usDelay == 0) |
julientiron | 0:cba942f8172a | 876 | return; |
julientiron | 0:cba942f8172a | 877 | |
julientiron | 0:cba942f8172a | 878 | // the following loop takes a 1/5 of a microsecond (4 cycles) |
julientiron | 0:cba942f8172a | 879 | // per iteration, so execute it five times for each microsecond of |
julientiron | 0:cba942f8172a | 880 | // delay requested. |
julientiron | 0:cba942f8172a | 881 | usDelay = (usDelay<<2) + usDelay; // x5 us |
julientiron | 0:cba942f8172a | 882 | |
julientiron | 0:cba942f8172a | 883 | // account for the time taken in the preceeding commands. |
julientiron | 0:cba942f8172a | 884 | usDelay -= 2; |
julientiron | 0:cba942f8172a | 885 | |
julientiron | 0:cba942f8172a | 886 | #elif F_CPU >= 16000000L |
julientiron | 0:cba942f8172a | 887 | // for the 16 MHz clock on most Arduino boards |
julientiron | 0:cba942f8172a | 888 | |
julientiron | 0:cba942f8172a | 889 | // for a one-microsecond delay, simply return. the overhead |
julientiron | 0:cba942f8172a | 890 | // of the function call yields a delay of approximately 1 1/8 us. |
julientiron | 0:cba942f8172a | 891 | if (--usDelay == 0) |
julientiron | 0:cba942f8172a | 892 | return; |
julientiron | 0:cba942f8172a | 893 | |
julientiron | 0:cba942f8172a | 894 | // the following loop takes a quarter of a microsecond (4 cycles) |
julientiron | 0:cba942f8172a | 895 | // per iteration, so execute it four times for each microsecond of |
julientiron | 0:cba942f8172a | 896 | // delay requested. |
julientiron | 0:cba942f8172a | 897 | usDelay <<= 2; |
julientiron | 0:cba942f8172a | 898 | |
julientiron | 0:cba942f8172a | 899 | // account for the time taken in the preceeding commands. |
julientiron | 0:cba942f8172a | 900 | usDelay -= 2; |
julientiron | 0:cba942f8172a | 901 | #else |
julientiron | 0:cba942f8172a | 902 | // for the 8 MHz internal clock on the ATmega168 |
julientiron | 0:cba942f8172a | 903 | |
julientiron | 0:cba942f8172a | 904 | // for a one- or two-microsecond delay, simply return. the overhead of |
julientiron | 0:cba942f8172a | 905 | // the function calls takes more than two microseconds. can't just |
julientiron | 0:cba942f8172a | 906 | // subtract two, since us is unsigned; we'd overflow. |
julientiron | 0:cba942f8172a | 907 | if (--usDelay == 0) |
julientiron | 0:cba942f8172a | 908 | return; |
julientiron | 0:cba942f8172a | 909 | if (--usDelay == 0) |
julientiron | 0:cba942f8172a | 910 | return; |
julientiron | 0:cba942f8172a | 911 | |
julientiron | 0:cba942f8172a | 912 | // the following loop takes half of a microsecond (4 cycles) |
julientiron | 0:cba942f8172a | 913 | // per iteration, so execute it twice for each microsecond of |
julientiron | 0:cba942f8172a | 914 | // delay requested. |
julientiron | 0:cba942f8172a | 915 | usDelay <<= 1; |
julientiron | 0:cba942f8172a | 916 | |
julientiron | 0:cba942f8172a | 917 | // partially compensate for the time taken by the preceeding commands. |
julientiron | 0:cba942f8172a | 918 | // we can't subtract any more than this or we'd overflow w/ small delays. |
julientiron | 0:cba942f8172a | 919 | usDelay--; |
julientiron | 0:cba942f8172a | 920 | #endif |
julientiron | 0:cba942f8172a | 921 | } |
julientiron | 0:cba942f8172a | 922 | |
julientiron | 0:cba942f8172a | 923 | /******************************************************//** |
julientiron | 0:cba942f8172a | 924 | * @brief Gets the pointer to the Easyspin instance |
julientiron | 0:cba942f8172a | 925 | * @param None |
julientiron | 0:cba942f8172a | 926 | * @retval Pointer to the instance of Easyspin |
julientiron | 0:cba942f8172a | 927 | **********************************************************/ |
julientiron | 0:cba942f8172a | 928 | class Easyspin* Easyspin::GetInstancePtr(void) |
julientiron | 0:cba942f8172a | 929 | { |
julientiron | 0:cba942f8172a | 930 | return (class Easyspin*)instancePtr; |
julientiron | 0:cba942f8172a | 931 | } |
julientiron | 0:cba942f8172a | 932 | |
julientiron | 0:cba942f8172a | 933 | /******************************************************//** |
julientiron | 0:cba942f8172a | 934 | * @brief Handles the shield state machine at each ste |
julientiron | 0:cba942f8172a | 935 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 936 | * @retval None |
julientiron | 0:cba942f8172a | 937 | * @note Must only be called by the timer ISR |
julientiron | 0:cba942f8172a | 938 | **********************************************************/ |
julientiron | 0:cba942f8172a | 939 | void Easyspin::StepClockHandler(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 940 | { |
julientiron | 0:cba942f8172a | 941 | /* Set isr flag */ |
julientiron | 0:cba942f8172a | 942 | isrFlag = true; |
julientiron | 0:cba942f8172a | 943 | |
julientiron | 0:cba942f8172a | 944 | /* Incrementation of the relative position */ |
julientiron | 0:cba942f8172a | 945 | shieldPrm[shieldId].relativePos++; |
julientiron | 0:cba942f8172a | 946 | |
julientiron | 0:cba942f8172a | 947 | /* Periodically check that estimated position is correct */ |
julientiron | 0:cba942f8172a | 948 | if ((shieldPrm[shieldId].commandExecuted != RUN_CMD) && |
julientiron | 0:cba942f8172a | 949 | ((shieldPrm[shieldId].relativePos % 10) == 00)) |
julientiron | 0:cba942f8172a | 950 | { |
julientiron | 0:cba942f8172a | 951 | uint32_t AbsPos= ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS)); |
julientiron | 0:cba942f8172a | 952 | |
julientiron | 0:cba942f8172a | 953 | /* Correct estimated position if needed */ |
julientiron | 0:cba942f8172a | 954 | if (AbsPos != 0) |
julientiron | 0:cba942f8172a | 955 | { |
julientiron | 0:cba942f8172a | 956 | if ((shieldPrm[shieldId].direction == FORWARD) && |
julientiron | 0:cba942f8172a | 957 | (AbsPos != shieldPrm[shieldId].currentPosition + shieldPrm[shieldId].relativePos)) |
julientiron | 0:cba942f8172a | 958 | { |
julientiron | 0:cba942f8172a | 959 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 960 | snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "F EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition)); |
julientiron | 0:cba942f8172a | 961 | Serial.println(EasyspinStrOut); |
julientiron | 0:cba942f8172a | 962 | #endif |
julientiron | 0:cba942f8172a | 963 | shieldPrm[shieldId].relativePos = AbsPos - shieldPrm[shieldId].currentPosition; |
julientiron | 0:cba942f8172a | 964 | |
julientiron | 0:cba942f8172a | 965 | } |
julientiron | 0:cba942f8172a | 966 | else if ((shieldPrm[shieldId].direction == BACKWARD) && |
julientiron | 0:cba942f8172a | 967 | (AbsPos != shieldPrm[shieldId].currentPosition - shieldPrm[shieldId].relativePos)) |
julientiron | 0:cba942f8172a | 968 | { |
julientiron | 0:cba942f8172a | 969 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 970 | snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "B EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition)); |
julientiron | 0:cba942f8172a | 971 | Serial.println(EasyspinStrOut); |
julientiron | 0:cba942f8172a | 972 | #endif |
julientiron | 0:cba942f8172a | 973 | shieldPrm[shieldId].relativePos = shieldPrm[shieldId].currentPosition - AbsPos; |
julientiron | 0:cba942f8172a | 974 | } |
julientiron | 0:cba942f8172a | 975 | } |
julientiron | 0:cba942f8172a | 976 | } |
julientiron | 0:cba942f8172a | 977 | |
julientiron | 0:cba942f8172a | 978 | switch (shieldPrm[shieldId].motionState) |
julientiron | 0:cba942f8172a | 979 | { |
julientiron | 0:cba942f8172a | 980 | case ACCELERATING: |
julientiron | 0:cba942f8172a | 981 | { |
julientiron | 0:cba942f8172a | 982 | if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)|| |
julientiron | 0:cba942f8172a | 983 | ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& |
julientiron | 0:cba942f8172a | 984 | (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].startDecPos))) |
julientiron | 0:cba942f8172a | 985 | { |
julientiron | 0:cba942f8172a | 986 | shieldPrm[shieldId].motionState = DECELERATING; |
julientiron | 0:cba942f8172a | 987 | shieldPrm[shieldId].accu = 0; |
julientiron | 0:cba942f8172a | 988 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 989 | snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Dec: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos); |
julientiron | 0:cba942f8172a | 990 | Serial.println(EasyspinStrOut); |
julientiron | 0:cba942f8172a | 991 | #endif |
julientiron | 0:cba942f8172a | 992 | } |
julientiron | 0:cba942f8172a | 993 | else if ((shieldPrm[shieldId].speed >= shieldPrm[shieldId].maxSpeed)|| |
julientiron | 0:cba942f8172a | 994 | ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& |
julientiron | 0:cba942f8172a | 995 | (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].endAccPos))) |
julientiron | 0:cba942f8172a | 996 | { |
julientiron | 0:cba942f8172a | 997 | shieldPrm[shieldId].motionState = STEADY; |
julientiron | 0:cba942f8172a | 998 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 999 | snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos); |
julientiron | 0:cba942f8172a | 1000 | Serial.println(EasyspinStrOut); |
julientiron | 0:cba942f8172a | 1001 | #endif |
julientiron | 0:cba942f8172a | 1002 | } |
julientiron | 0:cba942f8172a | 1003 | else |
julientiron | 0:cba942f8172a | 1004 | { |
julientiron | 0:cba942f8172a | 1005 | bool speedUpdated = false; |
julientiron | 0:cba942f8172a | 1006 | /* Go on accelerating */ |
julientiron | 0:cba942f8172a | 1007 | shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].acceleration << 16) / shieldPrm[shieldId].speed; |
julientiron | 0:cba942f8172a | 1008 | while (shieldPrm[shieldId].accu >= (0X10000L)) |
julientiron | 0:cba942f8172a | 1009 | { |
julientiron | 0:cba942f8172a | 1010 | shieldPrm[shieldId].accu -= (0X10000L); |
julientiron | 0:cba942f8172a | 1011 | shieldPrm[shieldId].speed +=1; |
julientiron | 0:cba942f8172a | 1012 | speedUpdated = true; |
julientiron | 0:cba942f8172a | 1013 | } |
julientiron | 0:cba942f8172a | 1014 | |
julientiron | 0:cba942f8172a | 1015 | if (speedUpdated) |
julientiron | 0:cba942f8172a | 1016 | { |
julientiron | 0:cba942f8172a | 1017 | ApplySpeed(shieldId, shieldPrm[shieldId].speed); |
julientiron | 0:cba942f8172a | 1018 | } |
julientiron | 0:cba942f8172a | 1019 | } |
julientiron | 0:cba942f8172a | 1020 | break; |
julientiron | 0:cba942f8172a | 1021 | } |
julientiron | 0:cba942f8172a | 1022 | case STEADY: |
julientiron | 0:cba942f8172a | 1023 | { |
julientiron | 0:cba942f8172a | 1024 | if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)|| |
julientiron | 0:cba942f8172a | 1025 | ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& |
julientiron | 0:cba942f8172a | 1026 | (shieldPrm[shieldId].relativePos >= (shieldPrm[shieldId].startDecPos))) || |
julientiron | 0:cba942f8172a | 1027 | ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&& |
julientiron | 0:cba942f8172a | 1028 | (shieldPrm[shieldId].speed > shieldPrm[shieldId].maxSpeed))) |
julientiron | 0:cba942f8172a | 1029 | { |
julientiron | 0:cba942f8172a | 1030 | shieldPrm[shieldId].motionState = DECELERATING; |
julientiron | 0:cba942f8172a | 1031 | shieldPrm[shieldId].accu = 0; |
julientiron | 0:cba942f8172a | 1032 | } |
julientiron | 0:cba942f8172a | 1033 | else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&& |
julientiron | 0:cba942f8172a | 1034 | (shieldPrm[shieldId].speed < shieldPrm[shieldId].maxSpeed)) |
julientiron | 0:cba942f8172a | 1035 | { |
julientiron | 0:cba942f8172a | 1036 | shieldPrm[shieldId].motionState = ACCELERATING; |
julientiron | 0:cba942f8172a | 1037 | shieldPrm[shieldId].accu = 0; |
julientiron | 0:cba942f8172a | 1038 | } |
julientiron | 0:cba942f8172a | 1039 | break; |
julientiron | 0:cba942f8172a | 1040 | } |
julientiron | 0:cba942f8172a | 1041 | case DECELERATING: |
julientiron | 0:cba942f8172a | 1042 | { |
julientiron | 0:cba942f8172a | 1043 | if (((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)&&(shieldPrm[shieldId].speed <= shieldPrm[shieldId].minSpeed))|| |
julientiron | 0:cba942f8172a | 1044 | ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&& |
julientiron | 0:cba942f8172a | 1045 | (shieldPrm[shieldId].relativePos >= shieldPrm[shieldId].stepsToTake))) |
julientiron | 0:cba942f8172a | 1046 | { |
julientiron | 0:cba942f8172a | 1047 | /* Motion process complete */ |
julientiron | 0:cba942f8172a | 1048 | HardStop(shieldId); |
julientiron | 0:cba942f8172a | 1049 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 1050 | snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Stop: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos ); |
julientiron | 0:cba942f8172a | 1051 | Serial.println(EasyspinStrOut); |
julientiron | 0:cba942f8172a | 1052 | #endif |
julientiron | 0:cba942f8172a | 1053 | } |
julientiron | 0:cba942f8172a | 1054 | else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&& |
julientiron | 0:cba942f8172a | 1055 | (shieldPrm[shieldId].speed <= shieldPrm[shieldId].maxSpeed)) |
julientiron | 0:cba942f8172a | 1056 | { |
julientiron | 0:cba942f8172a | 1057 | shieldPrm[shieldId].motionState = STEADY; |
julientiron | 0:cba942f8172a | 1058 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 1059 | snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos); |
julientiron | 0:cba942f8172a | 1060 | Serial.println(EasyspinStrOut); |
julientiron | 0:cba942f8172a | 1061 | #endif |
julientiron | 0:cba942f8172a | 1062 | } |
julientiron | 0:cba942f8172a | 1063 | else |
julientiron | 0:cba942f8172a | 1064 | { |
julientiron | 0:cba942f8172a | 1065 | /* Go on decelerating */ |
julientiron | 0:cba942f8172a | 1066 | if (shieldPrm[shieldId].speed > shieldPrm[shieldId].minSpeed) |
julientiron | 0:cba942f8172a | 1067 | { |
julientiron | 0:cba942f8172a | 1068 | bool speedUpdated = false; |
julientiron | 0:cba942f8172a | 1069 | shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].deceleration << 16) / shieldPrm[shieldId].speed; |
julientiron | 0:cba942f8172a | 1070 | while (shieldPrm[shieldId].accu >= (0X10000L)) |
julientiron | 0:cba942f8172a | 1071 | { |
julientiron | 0:cba942f8172a | 1072 | shieldPrm[shieldId].accu -= (0X10000L); |
julientiron | 0:cba942f8172a | 1073 | shieldPrm[shieldId].speed -=1; |
julientiron | 0:cba942f8172a | 1074 | speedUpdated = true; |
julientiron | 0:cba942f8172a | 1075 | } |
julientiron | 0:cba942f8172a | 1076 | if (speedUpdated) |
julientiron | 0:cba942f8172a | 1077 | { |
julientiron | 0:cba942f8172a | 1078 | ApplySpeed(shieldId, shieldPrm[shieldId].speed); |
julientiron | 0:cba942f8172a | 1079 | } |
julientiron | 0:cba942f8172a | 1080 | } |
julientiron | 0:cba942f8172a | 1081 | } |
julientiron | 0:cba942f8172a | 1082 | break; |
julientiron | 0:cba942f8172a | 1083 | } |
julientiron | 0:cba942f8172a | 1084 | default: |
julientiron | 0:cba942f8172a | 1085 | { |
julientiron | 0:cba942f8172a | 1086 | break; |
julientiron | 0:cba942f8172a | 1087 | } |
julientiron | 0:cba942f8172a | 1088 | } |
julientiron | 0:cba942f8172a | 1089 | /* Set isr flag */ |
julientiron | 0:cba942f8172a | 1090 | isrFlag = false; |
julientiron | 0:cba942f8172a | 1091 | } |
julientiron | 0:cba942f8172a | 1092 | |
julientiron | 0:cba942f8172a | 1093 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1094 | * @brief Updates the current speed of the shield |
julientiron | 0:cba942f8172a | 1095 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 1096 | * @param[in] newSpeed in pps |
julientiron | 0:cba942f8172a | 1097 | * @retval None |
julientiron | 0:cba942f8172a | 1098 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1099 | void Easyspin::ApplySpeed(uint8_t shieldId, uint16_t newSpeed) |
julientiron | 0:cba942f8172a | 1100 | { |
julientiron | 0:cba942f8172a | 1101 | if (newSpeed < Easyspin_MIN_PWM_FREQ) |
julientiron | 0:cba942f8172a | 1102 | { |
julientiron | 0:cba942f8172a | 1103 | newSpeed = Easyspin_MIN_PWM_FREQ; |
julientiron | 0:cba942f8172a | 1104 | } |
julientiron | 0:cba942f8172a | 1105 | if (newSpeed > Easyspin_MAX_PWM_FREQ) |
julientiron | 0:cba942f8172a | 1106 | { |
julientiron | 0:cba942f8172a | 1107 | newSpeed = Easyspin_MAX_PWM_FREQ; |
julientiron | 0:cba942f8172a | 1108 | } |
julientiron | 0:cba942f8172a | 1109 | |
julientiron | 0:cba942f8172a | 1110 | shieldPrm[shieldId].speed = newSpeed; |
julientiron | 0:cba942f8172a | 1111 | |
julientiron | 0:cba942f8172a | 1112 | switch (shieldId) |
julientiron | 0:cba942f8172a | 1113 | { |
julientiron | 0:cba942f8172a | 1114 | case 0: |
julientiron | 0:cba942f8172a | 1115 | Pwm1SetFreq(newSpeed); |
julientiron | 0:cba942f8172a | 1116 | break; |
julientiron | 0:cba942f8172a | 1117 | case 1: |
julientiron | 0:cba942f8172a | 1118 | Pwm2SetFreq(newSpeed); |
julientiron | 0:cba942f8172a | 1119 | break; |
julientiron | 0:cba942f8172a | 1120 | case 2: |
julientiron | 0:cba942f8172a | 1121 | Pwm3SetFreq(newSpeed); |
julientiron | 0:cba942f8172a | 1122 | break; |
julientiron | 0:cba942f8172a | 1123 | default: |
julientiron | 0:cba942f8172a | 1124 | break; //ignore error |
julientiron | 0:cba942f8172a | 1125 | } |
julientiron | 0:cba942f8172a | 1126 | } |
julientiron | 0:cba942f8172a | 1127 | |
julientiron | 0:cba942f8172a | 1128 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1129 | * @brief Computes the speed profile according to the number of steps to move |
julientiron | 0:cba942f8172a | 1130 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 1131 | * @param[in] nbSteps number of steps to perform |
julientiron | 0:cba942f8172a | 1132 | * @retval None |
julientiron | 0:cba942f8172a | 1133 | * @note Using the acceleration and deceleration of the shield, |
julientiron | 0:cba942f8172a | 1134 | * this function determines the duration in steps of the acceleration, |
julientiron | 0:cba942f8172a | 1135 | * steady and deceleration phases. |
julientiron | 0:cba942f8172a | 1136 | * If the total number of steps to perform is big enough, a trapezoidal move |
julientiron | 0:cba942f8172a | 1137 | * is performed (i.e. there is a steady phase where the motor runs at the maximum |
julientiron | 0:cba942f8172a | 1138 | * speed. |
julientiron | 0:cba942f8172a | 1139 | * Else, a triangular move is performed (no steady phase: the maximum speed is never |
julientiron | 0:cba942f8172a | 1140 | * reached. |
julientiron | 0:cba942f8172a | 1141 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1142 | void Easyspin::ComputeSpeedProfile(uint8_t shieldId, uint32_t nbSteps) |
julientiron | 0:cba942f8172a | 1143 | { |
julientiron | 0:cba942f8172a | 1144 | uint32_t reqAccSteps; |
julientiron | 0:cba942f8172a | 1145 | uint32_t reqDecSteps; |
julientiron | 0:cba942f8172a | 1146 | |
julientiron | 0:cba942f8172a | 1147 | /* compute the number of steps to get the targeted speed */ |
julientiron | 0:cba942f8172a | 1148 | reqAccSteps = (shieldPrm[shieldId].maxSpeed - shieldPrm[shieldId].minSpeed); |
julientiron | 0:cba942f8172a | 1149 | reqAccSteps *= (shieldPrm[shieldId].maxSpeed + shieldPrm[shieldId].minSpeed); |
julientiron | 0:cba942f8172a | 1150 | reqDecSteps = reqAccSteps; |
julientiron | 0:cba942f8172a | 1151 | reqAccSteps /= (uint32_t)shieldPrm[shieldId].acceleration; |
julientiron | 0:cba942f8172a | 1152 | reqAccSteps /= 2; |
julientiron | 0:cba942f8172a | 1153 | |
julientiron | 0:cba942f8172a | 1154 | /* compute the number of steps to stop */ |
julientiron | 0:cba942f8172a | 1155 | reqDecSteps /= (uint32_t)shieldPrm[shieldId].deceleration; |
julientiron | 0:cba942f8172a | 1156 | reqDecSteps /= 2; |
julientiron | 0:cba942f8172a | 1157 | |
julientiron | 0:cba942f8172a | 1158 | if(( reqAccSteps + reqDecSteps ) > nbSteps) |
julientiron | 0:cba942f8172a | 1159 | { |
julientiron | 0:cba942f8172a | 1160 | /* Triangular move */ |
julientiron | 0:cba942f8172a | 1161 | /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */ |
julientiron | 0:cba942f8172a | 1162 | |
julientiron | 0:cba942f8172a | 1163 | reqDecSteps = ((uint32_t) shieldPrm[shieldId].deceleration * nbSteps) / (shieldPrm[shieldId].acceleration + shieldPrm[shieldId].deceleration); |
julientiron | 0:cba942f8172a | 1164 | if (reqDecSteps > 1) |
julientiron | 0:cba942f8172a | 1165 | { |
julientiron | 0:cba942f8172a | 1166 | reqAccSteps = reqDecSteps - 1; |
julientiron | 0:cba942f8172a | 1167 | if(reqAccSteps == 0) |
julientiron | 0:cba942f8172a | 1168 | { |
julientiron | 0:cba942f8172a | 1169 | reqAccSteps = 1; |
julientiron | 0:cba942f8172a | 1170 | } |
julientiron | 0:cba942f8172a | 1171 | } |
julientiron | 0:cba942f8172a | 1172 | else |
julientiron | 0:cba942f8172a | 1173 | { |
julientiron | 0:cba942f8172a | 1174 | reqAccSteps = 0; |
julientiron | 0:cba942f8172a | 1175 | } |
julientiron | 0:cba942f8172a | 1176 | shieldPrm[shieldId].endAccPos = reqAccSteps; |
julientiron | 0:cba942f8172a | 1177 | shieldPrm[shieldId].startDecPos = reqDecSteps; |
julientiron | 0:cba942f8172a | 1178 | } |
julientiron | 0:cba942f8172a | 1179 | else |
julientiron | 0:cba942f8172a | 1180 | { |
julientiron | 0:cba942f8172a | 1181 | /* Trapezoidal move */ |
julientiron | 0:cba942f8172a | 1182 | /* accelerating phase to endAccPos */ |
julientiron | 0:cba942f8172a | 1183 | /* steady phase from endAccPos to startDecPos */ |
julientiron | 0:cba942f8172a | 1184 | /* decelerating from startDecPos to stepsToTake*/ |
julientiron | 0:cba942f8172a | 1185 | shieldPrm[shieldId].endAccPos = reqAccSteps; |
julientiron | 0:cba942f8172a | 1186 | shieldPrm[shieldId].startDecPos = nbSteps - reqDecSteps - 1; |
julientiron | 0:cba942f8172a | 1187 | } |
julientiron | 0:cba942f8172a | 1188 | } |
julientiron | 0:cba942f8172a | 1189 | |
julientiron | 0:cba942f8172a | 1190 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1191 | * @brief Converts the ABS_POSITION register value to a 32b signed integer |
julientiron | 0:cba942f8172a | 1192 | * @param[in] abs_position_reg value of the ABS_POSITION register |
julientiron | 0:cba942f8172a | 1193 | * @retval operation_result 32b signed integer corresponding to the absolute position |
julientiron | 0:cba942f8172a | 1194 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1195 | int32_t Easyspin::ConvertPosition(uint32_t abs_position_reg) |
julientiron | 0:cba942f8172a | 1196 | { |
julientiron | 0:cba942f8172a | 1197 | int32_t operation_result; |
julientiron | 0:cba942f8172a | 1198 | |
julientiron | 0:cba942f8172a | 1199 | if (abs_position_reg & Easyspin_ABS_POS_SIGN_BIT_MASK) |
julientiron | 0:cba942f8172a | 1200 | { |
julientiron | 0:cba942f8172a | 1201 | /* Negative register value */ |
julientiron | 0:cba942f8172a | 1202 | abs_position_reg = ~abs_position_reg; |
julientiron | 0:cba942f8172a | 1203 | abs_position_reg += 1; |
julientiron | 0:cba942f8172a | 1204 | |
julientiron | 0:cba942f8172a | 1205 | operation_result = (int32_t) (abs_position_reg & Easyspin_ABS_POS_VALUE_MASK); |
julientiron | 0:cba942f8172a | 1206 | operation_result = -operation_result; |
julientiron | 0:cba942f8172a | 1207 | } |
julientiron | 0:cba942f8172a | 1208 | else |
julientiron | 0:cba942f8172a | 1209 | { |
julientiron | 0:cba942f8172a | 1210 | operation_result = (int32_t) abs_position_reg; |
julientiron | 0:cba942f8172a | 1211 | } |
julientiron | 0:cba942f8172a | 1212 | return operation_result; |
julientiron | 0:cba942f8172a | 1213 | } |
julientiron | 0:cba942f8172a | 1214 | |
julientiron | 0:cba942f8172a | 1215 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1216 | * @brief Handlers of the flag interrupt which calls the user callback (if defined) |
julientiron | 0:cba942f8172a | 1217 | * @param None |
julientiron | 0:cba942f8172a | 1218 | * @retval None |
julientiron | 0:cba942f8172a | 1219 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1220 | void Easyspin::FlagInterruptHandler(void) |
julientiron | 0:cba942f8172a | 1221 | { |
julientiron | 0:cba942f8172a | 1222 | if (flagInterruptCallback != NULL) |
julientiron | 0:cba942f8172a | 1223 | { |
julientiron | 0:cba942f8172a | 1224 | /* Set isr flag */ |
julientiron | 0:cba942f8172a | 1225 | isrFlag = true; |
julientiron | 0:cba942f8172a | 1226 | |
julientiron | 0:cba942f8172a | 1227 | flagInterruptCallback(); |
julientiron | 0:cba942f8172a | 1228 | |
julientiron | 0:cba942f8172a | 1229 | /* Reset isr flag */ |
julientiron | 0:cba942f8172a | 1230 | isrFlag = false; |
julientiron | 0:cba942f8172a | 1231 | } |
julientiron | 0:cba942f8172a | 1232 | } |
julientiron | 0:cba942f8172a | 1233 | |
julientiron | 0:cba942f8172a | 1234 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1235 | * @brief Sends a command without arguments to the Easyspin via the SPI |
julientiron | 0:cba942f8172a | 1236 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 1237 | * @param[in] param Command to send |
julientiron | 0:cba942f8172a | 1238 | * @retval None |
julientiron | 0:cba942f8172a | 1239 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1240 | void Easyspin::SendCommand(uint8_t shieldId, uint8_t param) |
julientiron | 0:cba942f8172a | 1241 | { |
julientiron | 0:cba942f8172a | 1242 | uint8_t spiIndex = numberOfShields - shieldId - 1; |
julientiron | 0:cba942f8172a | 1243 | bool itDisable = false; |
julientiron | 0:cba942f8172a | 1244 | |
julientiron | 0:cba942f8172a | 1245 | do |
julientiron | 0:cba942f8172a | 1246 | { |
julientiron | 0:cba942f8172a | 1247 | spiPreemtionByIsr = false; |
julientiron | 0:cba942f8172a | 1248 | if (itDisable) |
julientiron | 0:cba942f8172a | 1249 | { |
julientiron | 0:cba942f8172a | 1250 | /* re-enable interrupts if disable in previous iteration */ |
julientiron | 0:cba942f8172a | 1251 | flag.enable_irq(); |
julientiron | 0:cba942f8172a | 1252 | itDisable = false; |
julientiron | 0:cba942f8172a | 1253 | } |
julientiron | 0:cba942f8172a | 1254 | |
julientiron | 0:cba942f8172a | 1255 | for (uint32_t i = 0; i < numberOfShields; i++) |
julientiron | 0:cba942f8172a | 1256 | { |
julientiron | 0:cba942f8172a | 1257 | spiTxBursts[3][i] = Easyspin_NOP; |
julientiron | 0:cba942f8172a | 1258 | } |
julientiron | 0:cba942f8172a | 1259 | spiTxBursts[3][spiIndex] = param; |
julientiron | 0:cba942f8172a | 1260 | |
julientiron | 0:cba942f8172a | 1261 | /* Disable interruption before checking */ |
julientiron | 0:cba942f8172a | 1262 | /* pre-emption by ISR and SPI transfers*/ |
julientiron | 0:cba942f8172a | 1263 | flag.disable_irq(); |
julientiron | 0:cba942f8172a | 1264 | itDisable = true; |
julientiron | 0:cba942f8172a | 1265 | } while (spiPreemtionByIsr); // check pre-emption by ISR |
julientiron | 0:cba942f8172a | 1266 | |
julientiron | 0:cba942f8172a | 1267 | WriteBytes(&spiTxBursts[3][0], &spiRxBursts[3][0]); |
julientiron | 0:cba942f8172a | 1268 | |
julientiron | 0:cba942f8172a | 1269 | /* re-enable interrupts after SPI transfers*/ |
julientiron | 0:cba942f8172a | 1270 | flag.enable_irq(); |
julientiron | 0:cba942f8172a | 1271 | } |
julientiron | 0:cba942f8172a | 1272 | |
julientiron | 0:cba942f8172a | 1273 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1274 | * @brief Sets the registers of the Easyspin to their predefined values |
julientiron | 0:cba942f8172a | 1275 | * from Easyspin_target_config.h |
julientiron | 0:cba942f8172a | 1276 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 1277 | * @retval None |
julientiron | 0:cba942f8172a | 1278 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1279 | void Easyspin::SetRegisterToPredefinedValues(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 1280 | { |
julientiron | 0:cba942f8172a | 1281 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1282 | Easyspin_ABS_POS, |
julientiron | 0:cba942f8172a | 1283 | 0); |
julientiron | 0:cba942f8172a | 1284 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1285 | Easyspin_EL_POS, |
julientiron | 0:cba942f8172a | 1286 | 0); |
julientiron | 0:cba942f8172a | 1287 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1288 | Easyspin_MARK, |
julientiron | 0:cba942f8172a | 1289 | 0); |
julientiron | 0:cba942f8172a | 1290 | switch (shieldId) |
julientiron | 0:cba942f8172a | 1291 | { |
julientiron | 0:cba942f8172a | 1292 | case 0: |
julientiron | 0:cba942f8172a | 1293 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1294 | Easyspin_TVAL, |
julientiron | 0:cba942f8172a | 1295 | Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_0)); |
julientiron | 0:cba942f8172a | 1296 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1297 | Easyspin_T_FAST, |
julientiron | 0:cba942f8172a | 1298 | (uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_0 | |
julientiron | 0:cba942f8172a | 1299 | (uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_0); |
julientiron | 0:cba942f8172a | 1300 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1301 | Easyspin_TON_MIN, |
julientiron | 0:cba942f8172a | 1302 | Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_0)); |
julientiron | 0:cba942f8172a | 1303 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1304 | Easyspin_TOFF_MIN, |
julientiron | 0:cba942f8172a | 1305 | Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_0)); |
julientiron | 0:cba942f8172a | 1306 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1307 | Easyspin_OCD_TH, |
julientiron | 0:cba942f8172a | 1308 | Easyspin_CONF_PARAM_OCD_TH_SHIELD_0); |
julientiron | 0:cba942f8172a | 1309 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1310 | Easyspin_STEP_MODE, |
julientiron | 0:cba942f8172a | 1311 | (uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_0 | |
julientiron | 0:cba942f8172a | 1312 | (uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_0); |
julientiron | 0:cba942f8172a | 1313 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1314 | Easyspin_ALARM_EN, |
julientiron | 0:cba942f8172a | 1315 | Easyspin_CONF_PARAM_ALARM_EN_SHIELD_0); |
julientiron | 0:cba942f8172a | 1316 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1317 | Easyspin_CONFIG, |
julientiron | 0:cba942f8172a | 1318 | (uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_0 | |
julientiron | 0:cba942f8172a | 1319 | (uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_0 | |
julientiron | 0:cba942f8172a | 1320 | (uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_0 | |
julientiron | 0:cba942f8172a | 1321 | (uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_0 | |
julientiron | 0:cba942f8172a | 1322 | (uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_0); |
julientiron | 0:cba942f8172a | 1323 | break; |
julientiron | 0:cba942f8172a | 1324 | case 1: |
julientiron | 0:cba942f8172a | 1325 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1326 | Easyspin_TVAL, |
julientiron | 0:cba942f8172a | 1327 | Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_1)); |
julientiron | 0:cba942f8172a | 1328 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1329 | Easyspin_T_FAST, |
julientiron | 0:cba942f8172a | 1330 | (uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_1 | |
julientiron | 0:cba942f8172a | 1331 | (uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_1); |
julientiron | 0:cba942f8172a | 1332 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1333 | Easyspin_TON_MIN, |
julientiron | 0:cba942f8172a | 1334 | Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_1)); |
julientiron | 0:cba942f8172a | 1335 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1336 | Easyspin_TOFF_MIN, |
julientiron | 0:cba942f8172a | 1337 | Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_1)); |
julientiron | 0:cba942f8172a | 1338 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1339 | Easyspin_OCD_TH, |
julientiron | 0:cba942f8172a | 1340 | Easyspin_CONF_PARAM_OCD_TH_SHIELD_1); |
julientiron | 0:cba942f8172a | 1341 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1342 | Easyspin_STEP_MODE, |
julientiron | 0:cba942f8172a | 1343 | (uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_1 | |
julientiron | 0:cba942f8172a | 1344 | (uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_1); |
julientiron | 0:cba942f8172a | 1345 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1346 | Easyspin_ALARM_EN, |
julientiron | 0:cba942f8172a | 1347 | Easyspin_CONF_PARAM_ALARM_EN_SHIELD_1); |
julientiron | 0:cba942f8172a | 1348 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1349 | Easyspin_CONFIG, |
julientiron | 0:cba942f8172a | 1350 | (uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_1 | |
julientiron | 0:cba942f8172a | 1351 | (uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_1 | |
julientiron | 0:cba942f8172a | 1352 | (uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_1 | |
julientiron | 0:cba942f8172a | 1353 | (uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_1 | |
julientiron | 0:cba942f8172a | 1354 | (uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_1); |
julientiron | 0:cba942f8172a | 1355 | break; |
julientiron | 0:cba942f8172a | 1356 | case 2: |
julientiron | 0:cba942f8172a | 1357 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1358 | Easyspin_TVAL, |
julientiron | 0:cba942f8172a | 1359 | Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_2)); |
julientiron | 0:cba942f8172a | 1360 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1361 | Easyspin_T_FAST, |
julientiron | 0:cba942f8172a | 1362 | (uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_2 | |
julientiron | 0:cba942f8172a | 1363 | (uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_2); |
julientiron | 0:cba942f8172a | 1364 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1365 | Easyspin_TON_MIN, |
julientiron | 0:cba942f8172a | 1366 | Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_2)); |
julientiron | 0:cba942f8172a | 1367 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1368 | Easyspin_TOFF_MIN, |
julientiron | 0:cba942f8172a | 1369 | Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_2)); |
julientiron | 0:cba942f8172a | 1370 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1371 | Easyspin_OCD_TH, |
julientiron | 0:cba942f8172a | 1372 | Easyspin_CONF_PARAM_OCD_TH_SHIELD_2); |
julientiron | 0:cba942f8172a | 1373 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1374 | Easyspin_STEP_MODE, |
julientiron | 0:cba942f8172a | 1375 | (uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_2 | |
julientiron | 0:cba942f8172a | 1376 | (uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_2); |
julientiron | 0:cba942f8172a | 1377 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1378 | Easyspin_ALARM_EN, |
julientiron | 0:cba942f8172a | 1379 | Easyspin_CONF_PARAM_ALARM_EN_SHIELD_2); |
julientiron | 0:cba942f8172a | 1380 | CmdSetParam(shieldId, |
julientiron | 0:cba942f8172a | 1381 | Easyspin_CONFIG, |
julientiron | 0:cba942f8172a | 1382 | (uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_2 | |
julientiron | 0:cba942f8172a | 1383 | (uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_2 | |
julientiron | 0:cba942f8172a | 1384 | (uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_2 | |
julientiron | 0:cba942f8172a | 1385 | (uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_2 | |
julientiron | 0:cba942f8172a | 1386 | (uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_2); |
julientiron | 0:cba942f8172a | 1387 | break; |
julientiron | 0:cba942f8172a | 1388 | default: ; |
julientiron | 0:cba942f8172a | 1389 | } |
julientiron | 0:cba942f8172a | 1390 | } |
julientiron | 0:cba942f8172a | 1391 | |
julientiron | 0:cba942f8172a | 1392 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1393 | * @brief Sets the registers of the Easyspin to their predefined values |
julientiron | 0:cba942f8172a | 1394 | * from Easyspin_target_config.h |
julientiron | 0:cba942f8172a | 1395 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 1396 | * @retval None |
julientiron | 0:cba942f8172a | 1397 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1398 | void Easyspin::WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte) |
julientiron | 0:cba942f8172a | 1399 | { |
julientiron | 0:cba942f8172a | 1400 | CS = 0; |
julientiron | 0:cba942f8172a | 1401 | for (uint32_t i = 0; i < numberOfShields; i++) |
julientiron | 0:cba942f8172a | 1402 | { |
julientiron | 0:cba942f8172a | 1403 | *pReceivedByte = spi.write(*pByteToTransmit); |
julientiron | 0:cba942f8172a | 1404 | pByteToTransmit++; |
julientiron | 0:cba942f8172a | 1405 | pReceivedByte++; |
julientiron | 0:cba942f8172a | 1406 | } |
julientiron | 0:cba942f8172a | 1407 | CS = 1; |
julientiron | 0:cba942f8172a | 1408 | if (isrFlag) |
julientiron | 0:cba942f8172a | 1409 | { |
julientiron | 0:cba942f8172a | 1410 | spiPreemtionByIsr = true; |
julientiron | 0:cba942f8172a | 1411 | } |
julientiron | 0:cba942f8172a | 1412 | } |
julientiron | 0:cba942f8172a | 1413 | |
julientiron | 0:cba942f8172a | 1414 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1415 | * @brief Initialises the PWM uses by the specified shield |
julientiron | 0:cba942f8172a | 1416 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 1417 | * @retval None |
julientiron | 0:cba942f8172a | 1418 | * @note Shield 0 uses PW1 based on timer 1 |
julientiron | 0:cba942f8172a | 1419 | * Shield 1 uses PWM 2 based on timer 2 |
julientiron | 0:cba942f8172a | 1420 | * Shield 2 uses PWM3 based timer 0 |
julientiron | 0:cba942f8172a | 1421 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1422 | void Easyspin::PwmInit(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 1423 | { |
julientiron | 0:cba942f8172a | 1424 | switch (shieldId) |
julientiron | 0:cba942f8172a | 1425 | { |
julientiron | 0:cba942f8172a | 1426 | case 0: |
julientiron | 0:cba942f8172a | 1427 | /* PWM1 uses timer 1 */ |
julientiron | 0:cba942f8172a | 1428 | /* Initialise timer by setting waveform generation mode |
julientiron | 0:cba942f8172a | 1429 | to PWM phase and Frequency correct: mode = 8 |
julientiron | 0:cba942f8172a | 1430 | (WGM10 = 0, WGM11 = 0, WGM12 = 0, WGM13 = 1) */ |
julientiron | 0:cba942f8172a | 1431 | |
julientiron | 0:cba942f8172a | 1432 | /* Stop timer1 by clearing CSS bits and set WGM13 and WGM12 */ |
julientiron | 0:cba942f8172a | 1433 | //TCCR1B = 0x10; |
julientiron | 0:cba942f8172a | 1434 | |
julientiron | 0:cba942f8172a | 1435 | /* Set WGM10 and WGM11 */ |
julientiron | 0:cba942f8172a | 1436 | //TCCR1A = 0x00; |
julientiron | 0:cba942f8172a | 1437 | |
julientiron | 0:cba942f8172a | 1438 | /* Disable Timer1 interrupt */ |
julientiron | 0:cba942f8172a | 1439 | //TIMSK1 = 0; |
julientiron | 0:cba942f8172a | 1440 | //pwm1.period_us(400); |
julientiron | 0:cba942f8172a | 1441 | //pwm1.pulsewidth_us(200); |
julientiron | 0:cba942f8172a | 1442 | |
julientiron | 0:cba942f8172a | 1443 | break; |
julientiron | 0:cba942f8172a | 1444 | case 1: |
julientiron | 0:cba942f8172a | 1445 | /* PWM2 uses timer 2 */ |
julientiron | 0:cba942f8172a | 1446 | /* Initialise timer by setting waveform generation mode |
julientiron | 0:cba942f8172a | 1447 | to PWM phase correct: mode = 5 |
julientiron | 0:cba942f8172a | 1448 | (WGM0 = 1, WGM21 = 0, WGM22 = 1) */ |
julientiron | 0:cba942f8172a | 1449 | |
julientiron | 0:cba942f8172a | 1450 | /* Stop timer2 by clearing CSS bits and set WGM22 */ |
julientiron | 0:cba942f8172a | 1451 | //TCCR2B = 0x08; |
julientiron | 0:cba942f8172a | 1452 | |
julientiron | 0:cba942f8172a | 1453 | /* Set WGM20 and WGM21 */ |
julientiron | 0:cba942f8172a | 1454 | //TCCR2A = 0x01; |
julientiron | 0:cba942f8172a | 1455 | |
julientiron | 0:cba942f8172a | 1456 | /* Disable Timer2 interrupt */ |
julientiron | 0:cba942f8172a | 1457 | //TIMSK2 = 0; |
julientiron | 0:cba942f8172a | 1458 | //pwm2.period_us(500); |
julientiron | 0:cba942f8172a | 1459 | //pwm2.pulsewidth_us(100); |
julientiron | 0:cba942f8172a | 1460 | break; |
julientiron | 0:cba942f8172a | 1461 | |
julientiron | 0:cba942f8172a | 1462 | |
julientiron | 0:cba942f8172a | 1463 | case 2: |
julientiron | 0:cba942f8172a | 1464 | /* PWM3 uses timer 0 */ |
julientiron | 0:cba942f8172a | 1465 | /* !!!!! Caution: Calling this configuration will break */ |
julientiron | 0:cba942f8172a | 1466 | /* all default Arduino's timing functions as delay(),millis()... */ |
julientiron | 0:cba942f8172a | 1467 | |
julientiron | 0:cba942f8172a | 1468 | /* Initialise timer by setting waveform generation mode |
julientiron | 0:cba942f8172a | 1469 | to PWM phase correct: mode = 5 |
julientiron | 0:cba942f8172a | 1470 | (WGM0 = 1, WGM21 = 0, WGM22 = 1) */ |
julientiron | 0:cba942f8172a | 1471 | |
julientiron | 0:cba942f8172a | 1472 | /* Stop timer0 by clearing CSS bits and set WGM22 */ |
julientiron | 0:cba942f8172a | 1473 | //TCCR0B = 0x08; |
julientiron | 0:cba942f8172a | 1474 | |
julientiron | 0:cba942f8172a | 1475 | /* Set WGM00 and WGM01 */ |
julientiron | 0:cba942f8172a | 1476 | //TCCR0A = 0x01; |
julientiron | 0:cba942f8172a | 1477 | |
julientiron | 0:cba942f8172a | 1478 | /* Disable Timer0 interrupt */ |
julientiron | 0:cba942f8172a | 1479 | //TIMSK0 = 0; |
julientiron | 0:cba942f8172a | 1480 | //pwm3.period_ms(10); |
julientiron | 0:cba942f8172a | 1481 | //pwm3.pulsewidth_ms(1); |
julientiron | 0:cba942f8172a | 1482 | break; |
julientiron | 0:cba942f8172a | 1483 | default: |
julientiron | 0:cba942f8172a | 1484 | break;//ignore error |
julientiron | 0:cba942f8172a | 1485 | } |
julientiron | 0:cba942f8172a | 1486 | } |
julientiron | 0:cba942f8172a | 1487 | |
julientiron | 0:cba942f8172a | 1488 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1489 | * @brief Sets the frequency of PWM1 used by shield 0 |
julientiron | 0:cba942f8172a | 1490 | * @param[in] newFreq in Hz |
julientiron | 0:cba942f8172a | 1491 | * @retval None |
julientiron | 0:cba942f8172a | 1492 | * @note The frequency is directly the current speed of the shield |
julientiron | 0:cba942f8172a | 1493 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1494 | void Easyspin::Pwm1SetFreq(uint16_t newFreq) |
julientiron | 0:cba942f8172a | 1495 | { |
julientiron | 0:cba942f8172a | 1496 | pwm1.period_us(newFreq); |
julientiron | 0:cba942f8172a | 1497 | pwm1.pulsewidth_us(newFreq/2); |
julientiron | 0:cba942f8172a | 1498 | } |
julientiron | 0:cba942f8172a | 1499 | |
julientiron | 0:cba942f8172a | 1500 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1501 | * @brief Sets the frequency of PWM2 used by shield 1 |
julientiron | 0:cba942f8172a | 1502 | * @param[in] newFreq in Hz |
julientiron | 0:cba942f8172a | 1503 | * @retval None |
julientiron | 0:cba942f8172a | 1504 | * @note The frequency is directly the current speed of the shield |
julientiron | 0:cba942f8172a | 1505 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1506 | void Easyspin::Pwm2SetFreq(uint16_t newFreq) |
julientiron | 0:cba942f8172a | 1507 | { |
julientiron | 0:cba942f8172a | 1508 | pwm2.period_us(newFreq); |
julientiron | 0:cba942f8172a | 1509 | pwm2.pulsewidth_us(newFreq/2); |
julientiron | 0:cba942f8172a | 1510 | } |
julientiron | 0:cba942f8172a | 1511 | |
julientiron | 0:cba942f8172a | 1512 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1513 | * @brief Sets the frequency of PWM3 used by shield 2 |
julientiron | 0:cba942f8172a | 1514 | * @param[in] newFreq in Hz |
julientiron | 0:cba942f8172a | 1515 | * @retval None |
julientiron | 0:cba942f8172a | 1516 | * @note The frequency is directly the current speed of the shield |
julientiron | 0:cba942f8172a | 1517 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1518 | void Easyspin::Pwm3SetFreq(uint16_t newFreq) |
julientiron | 0:cba942f8172a | 1519 | { |
julientiron | 0:cba942f8172a | 1520 | pwm3.period_us(newFreq); |
julientiron | 0:cba942f8172a | 1521 | pwm3.pulsewidth_us(newFreq/2); |
julientiron | 0:cba942f8172a | 1522 | } |
julientiron | 0:cba942f8172a | 1523 | |
julientiron | 0:cba942f8172a | 1524 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1525 | * @brief Stops the PWM uses by the specified shield |
julientiron | 0:cba942f8172a | 1526 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 1527 | * @retval None |
julientiron | 0:cba942f8172a | 1528 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1529 | void Easyspin::PwmStop(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 1530 | { |
julientiron | 0:cba942f8172a | 1531 | switch (shieldId) |
julientiron | 0:cba942f8172a | 1532 | { |
julientiron | 0:cba942f8172a | 1533 | case 0: |
julientiron | 0:cba942f8172a | 1534 | pwm1.period_us(0); |
julientiron | 0:cba942f8172a | 1535 | pwm1.pulsewidth_us(0); |
julientiron | 0:cba942f8172a | 1536 | break; |
julientiron | 0:cba942f8172a | 1537 | case 1: |
julientiron | 0:cba942f8172a | 1538 | pwm2.period_us(0); |
julientiron | 0:cba942f8172a | 1539 | pwm2.pulsewidth_us(0); |
julientiron | 0:cba942f8172a | 1540 | break; |
julientiron | 0:cba942f8172a | 1541 | case 2: |
julientiron | 0:cba942f8172a | 1542 | pwm3.write(0.0f); |
julientiron | 0:cba942f8172a | 1543 | break; |
julientiron | 0:cba942f8172a | 1544 | default: |
julientiron | 0:cba942f8172a | 1545 | break;//ignore error |
julientiron | 0:cba942f8172a | 1546 | } |
julientiron | 0:cba942f8172a | 1547 | } |
julientiron | 0:cba942f8172a | 1548 | |
julientiron | 0:cba942f8172a | 1549 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1550 | * @brief Sets the parameters of the shield to predefined values |
julientiron | 0:cba942f8172a | 1551 | * from Easyspin_target_config.h |
julientiron | 0:cba942f8172a | 1552 | * @param None |
julientiron | 0:cba942f8172a | 1553 | * @retval None |
julientiron | 0:cba942f8172a | 1554 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1555 | void Easyspin::SetShieldParamsToPredefinedValues(void) |
julientiron | 0:cba942f8172a | 1556 | { |
julientiron | 0:cba942f8172a | 1557 | shieldPrm[0].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_0; |
julientiron | 0:cba942f8172a | 1558 | shieldPrm[0].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_0; |
julientiron | 0:cba942f8172a | 1559 | shieldPrm[0].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_0; |
julientiron | 0:cba942f8172a | 1560 | shieldPrm[0].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_0; |
julientiron | 0:cba942f8172a | 1561 | |
julientiron | 0:cba942f8172a | 1562 | shieldPrm[1].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_1; |
julientiron | 0:cba942f8172a | 1563 | shieldPrm[1].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_1; |
julientiron | 0:cba942f8172a | 1564 | shieldPrm[1].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_1; |
julientiron | 0:cba942f8172a | 1565 | shieldPrm[1].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_1; |
julientiron | 0:cba942f8172a | 1566 | |
julientiron | 0:cba942f8172a | 1567 | shieldPrm[2].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_2; |
julientiron | 0:cba942f8172a | 1568 | shieldPrm[2].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_2; |
julientiron | 0:cba942f8172a | 1569 | shieldPrm[2].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_2; |
julientiron | 0:cba942f8172a | 1570 | shieldPrm[2].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_2; |
julientiron | 0:cba942f8172a | 1571 | |
julientiron | 0:cba942f8172a | 1572 | for (uint8_t i = 0; i < numberOfShields; i++) |
julientiron | 0:cba942f8172a | 1573 | { |
julientiron | 0:cba942f8172a | 1574 | SetRegisterToPredefinedValues(i); |
julientiron | 0:cba942f8172a | 1575 | } |
julientiron | 0:cba942f8172a | 1576 | } |
julientiron | 0:cba942f8172a | 1577 | |
julientiron | 0:cba942f8172a | 1578 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1579 | * @brief Initialises the bridge parameters to start the movement |
julientiron | 0:cba942f8172a | 1580 | * and enable the power bridge |
julientiron | 0:cba942f8172a | 1581 | * @param[in] shieldId (from 0 to 2) |
julientiron | 0:cba942f8172a | 1582 | * @retval None |
julientiron | 0:cba942f8172a | 1583 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1584 | void Easyspin::StartMovement(uint8_t shieldId) |
julientiron | 0:cba942f8172a | 1585 | { |
julientiron | 0:cba942f8172a | 1586 | /* Enable Easyspin powerstage */ |
julientiron | 0:cba942f8172a | 1587 | CmdEnable(shieldId); |
julientiron | 0:cba942f8172a | 1588 | |
julientiron | 0:cba942f8172a | 1589 | if (shieldPrm[shieldId].endAccPos != 0) |
julientiron | 0:cba942f8172a | 1590 | { |
julientiron | 0:cba942f8172a | 1591 | shieldPrm[shieldId].motionState = ACCELERATING;; |
julientiron | 0:cba942f8172a | 1592 | } |
julientiron | 0:cba942f8172a | 1593 | else |
julientiron | 0:cba942f8172a | 1594 | { |
julientiron | 0:cba942f8172a | 1595 | shieldPrm[shieldId].motionState = DECELERATING; |
julientiron | 0:cba942f8172a | 1596 | } |
julientiron | 0:cba942f8172a | 1597 | |
julientiron | 0:cba942f8172a | 1598 | shieldPrm[shieldId].accu = 0; |
julientiron | 0:cba942f8172a | 1599 | shieldPrm[shieldId].relativePos = 0; |
julientiron | 0:cba942f8172a | 1600 | ApplySpeed(shieldId, shieldPrm[shieldId].minSpeed); |
julientiron | 0:cba942f8172a | 1601 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 1602 | snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Stop->Acc: speed: %u relPos: %ld\n", shieldPrm[shieldId].minSpeed, shieldPrm[shieldId].relativePos) ; |
julientiron | 0:cba942f8172a | 1603 | Serial.println(EasyspinStrOut); |
julientiron | 0:cba942f8172a | 1604 | #endif |
julientiron | 0:cba942f8172a | 1605 | } |
julientiron | 0:cba942f8172a | 1606 | |
julientiron | 0:cba942f8172a | 1607 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1608 | * @brief Converts mA in compatible values for TVAL register |
julientiron | 0:cba942f8172a | 1609 | * @param[in] Tval |
julientiron | 0:cba942f8172a | 1610 | * @retval TVAL values |
julientiron | 0:cba942f8172a | 1611 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1612 | inline uint8_t Easyspin::Tval_Current_to_Par(double Tval) |
julientiron | 0:cba942f8172a | 1613 | { |
julientiron | 0:cba942f8172a | 1614 | return ((uint8_t)(((Tval - 31.25)/31.25)+0.5)); |
julientiron | 0:cba942f8172a | 1615 | } |
julientiron | 0:cba942f8172a | 1616 | |
julientiron | 0:cba942f8172a | 1617 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1618 | * @brief Convert time in us in compatible values |
julientiron | 0:cba942f8172a | 1619 | * for TON_MIN register |
julientiron | 0:cba942f8172a | 1620 | * @param[in] Tmin |
julientiron | 0:cba942f8172a | 1621 | * @retval TON_MIN values |
julientiron | 0:cba942f8172a | 1622 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1623 | inline uint8_t Easyspin::Tmin_Time_to_Par(double Tmin) |
julientiron | 0:cba942f8172a | 1624 | { |
julientiron | 0:cba942f8172a | 1625 | return ((uint8_t)(((Tmin - 0.5)*2)+0.5)); |
julientiron | 0:cba942f8172a | 1626 | } |
julientiron | 0:cba942f8172a | 1627 | |
julientiron | 0:cba942f8172a | 1628 | |
julientiron | 0:cba942f8172a | 1629 | |
julientiron | 0:cba942f8172a | 1630 | /******************************************************//** |
julientiron | 0:cba942f8172a | 1631 | * @brief Debug function to get the amount of free ram |
julientiron | 0:cba942f8172a | 1632 | * @param None |
julientiron | 0:cba942f8172a | 1633 | * @retval number of bytes of free ram |
julientiron | 0:cba942f8172a | 1634 | **********************************************************/ |
julientiron | 0:cba942f8172a | 1635 | #ifdef _DEBUG_Easyspin |
julientiron | 0:cba942f8172a | 1636 | uint16_t GetFreeRam (void) |
julientiron | 0:cba942f8172a | 1637 | { |
julientiron | 0:cba942f8172a | 1638 | extern uint16_t __heap_start, *__brkval; |
julientiron | 0:cba942f8172a | 1639 | uint16_t v; |
julientiron | 0:cba942f8172a | 1640 | return (uint16_t) &v - (__brkval == 0 ? (uint16_t) &__heap_start : (uint16_t) __brkval); |
julientiron | 0:cba942f8172a | 1641 | } |
julientiron | 0:cba942f8172a | 1642 | #endif |