Robotique FIP
Speed profile working
Fork of
Easyspin_lib
by 
Julien Tiron
Diff: easyspin.cpp
- Revision:
- 1:9efe863db15e
- Parent:
- 0:cba942f8172a
--- a/easyspin.cpp Tue Jul 07 20:35:36 2015 +0000
+++ b/easyspin.cpp Wed Aug 26 13:35:55 2015 +0000
@@ -1,5 +1,5 @@
/******************************************************//**
- * @file easyspin.cpp
+ * @file easyspin.cpp
* @version V1.0
* @date June 29, 2015
* @brief easyspin library for mbed
@@ -8,16 +8,11 @@
* it under the terms of either the GNU General Public License version 2
* or the GNU Lesser General Public License version 2.1, both as
* published by the Free Software Foundation.
- **********************************************************/
+ **********************************************************/
#include "easyspin.h"
#include "mbed.h"
-#ifdef _DEBUG_Easyspin
-/// Log buffer
-char EasyspinStrOut[DEBUG_BUFFER_SIZE];
-#endif
-
const uint16_t Easyspin::prescalerArrayTimer0_1[PRESCALER_ARRAY_TIMER0_1_SIZE] = { 0, 1, 8, 64, 256, 1024};
const uint16_t Easyspin::prescalerArrayTimer2[PRESCALER_ARRAY_TIMER2_SIZE] = {0, 1, 8, 32, 64, 128, 256, 1024};
volatile void (*Easyspin::flagInterruptCallback)(void);
@@ -27,39 +22,38 @@
volatile bool Easyspin::spiPreemtionByIsr = false;
volatile bool Easyspin::isrFlag = false;
volatile class Easyspin* Easyspin::instancePtr = NULL;
-
+
/******************************************************//**
* @brief Constructor
* @param None
* @retval None
- **********************************************************/
+ **********************************************************/
Easyspin::Easyspin():reset(Easyspin_Reset_Pin), dir1(Easyspin_DIR_1_Pin), dir2(Easyspin_DIR_2_Pin),
- dir3(Easyspin_DIR_3_Pin), CS(Easyspin_CS_Pin), flag(Easyspin_FLAG_Pin), pwm1(Easyspin_PWM_1_Pin),
- pwm2(Easyspin_PWM_2_Pin), pwm3(Easyspin_PWM_3_Pin), spi(Easyspin_MOSI_Pin, Easyspin_MISO_Pin, Easyspin_SCK_Pin)
+ dir3(Easyspin_DIR_3_Pin), CS(Easyspin_CS_Pin), flag(Easyspin_FLAG_Pin), pwm1(Easyspin_PWM_1_Pin),
+ pwm2(Easyspin_PWM_2_Pin), pwm3(Easyspin_PWM_3_Pin), spi(Easyspin_MOSI_Pin, Easyspin_MISO_Pin, Easyspin_SCK_Pin)
{
- uint8_t i;
- for (i = 0; i < MAX_NUMBER_OF_SHIELDS; i++)
- {
- shieldPrm[i].motionState = INACTIVE;
- shieldPrm[i].commandExecuted = NO_CMD;
- shieldPrm[i].stepsToTake = MAX_STEPS;
- }
+ uint8_t i;
+ for (i = 0; i < MAX_NUMBER_OF_SHIELDS; i++) {
+ shieldPrm[i].motionState = INACTIVE;
+ shieldPrm[i].commandExecuted = NO_CMD;
+ shieldPrm[i].stepsToTake = MAX_STEPS;
+ }
instancePtr = this;
}
/******************************************************//**
* @brief Attaches a user callback to the flag Interrupt
- * The call back will be then called each time the status
- * flag pin will be pulled down due to the occurrence of
- * a programmed alarms ( OCD, thermal pre-warning or
+ * The call back will be then called each time the status
+ * flag pin will be pulled down due to the occurrence of
+ * a programmed alarms ( OCD, thermal pre-warning or
* shutdown, UVLO, wrong command, non-performable command)
- * @param[in] callback Name of the callback to attach
+ * @param[in] callback Name of the callback to attach
* to the Flag Interrupt
* @retval None
**********************************************************/
void Easyspin::AttachFlagInterrupt(void (*callback)(void))
{
- flagInterruptCallback = (volatile void (*)())callback;
+ flagInterruptCallback = (volatile void (*)())callback;
}
/******************************************************//**
@@ -69,52 +63,51 @@
**********************************************************/
void Easyspin::Begin(uint8_t nbShields)
{
- numberOfShields = nbShields;
-
- // start the SPI library:
- //SPI.begin();
- //SPI.setBitOrder(MSBFIRST);
- spi.format(8, 3);
- //SPI.setClockDivider(SPI_CLOCK_DIV4);
-
- // flag pin
- //pinMode(Easyspin_FLAG_Pin, INPUT_PULLUP);
- flag.fall(&FlagInterruptHandler);
-
- //reset pin
- //pinMode(Easyspin_Reset_Pin, OUTPUT);
-
- switch (nbShields)
- {
- case 3:
- //pinMode(Easyspin_DIR_3_Pin, OUTPUT);
- //pinMode(Easyspin_PWM_3_Pin, OUTPUT);
- PwmInit(2);
- case 2:
- //pinMode(Easyspin_DIR_2_Pin, OUTPUT);
- //pinMode(Easyspin_PWM_2_Pin, OUTPUT);
- PwmInit(1);
- case 1:
- //pinMode(Easyspin_DIR_1_Pin, OUTPUT);
- //pinMode(Easyspin_PWM_1_Pin, OUTPUT);
- PwmInit(0);
- default:
- ;
- }
-
- /* Standby-reset deactivation */
- ReleaseReset();
-
- /* Set all registers and context variables to the predefined values from Easyspin_target_config.h */
- SetShieldParamsToPredefinedValues();
-
- /* Disable Easyspin powerstage */
- for (uint32_t i = 0; i < nbShields; i++)
- {
- CmdDisable(i);
- /* Get Status to clear flags after start up */
- CmdGetStatus(i);
- }
+ numberOfShields = nbShields;
+
+ // start the SPI library:
+ //SPI.begin();
+ //SPI.setBitOrder(MSBFIRST);
+ spi.format(8, 3);
+ //SPI.setClockDivider(SPI_CLOCK_DIV4);
+
+ // flag pin
+ //pinMode(Easyspin_FLAG_Pin, INPUT_PULLUP);
+ flag.mode(PullUp);
+ flag.fall(&FlagInterruptHandler);
+
+ //reset pin
+ //pinMode(Easyspin_Reset_Pin, OUTPUT);
+
+ switch (nbShields) {
+ case 3:
+ //pinMode(Easyspin_DIR_3_Pin, OUTPUT);
+ //pinMode(Easyspin_PWM_3_Pin, OUTPUT);
+ PwmInit(2);
+ case 2:
+ //pinMode(Easyspin_DIR_2_Pin, OUTPUT);
+ //pinMode(Easyspin_PWM_2_Pin, OUTPUT);
+ PwmInit(1);
+ case 1:
+ //pinMode(Easyspin_DIR_1_Pin, OUTPUT);
+ //pinMode(Easyspin_PWM_1_Pin, OUTPUT);
+ PwmInit(0);
+ default:
+ ;
+ }
+
+ /* Standby-reset deactivation */
+ ReleaseReset();
+
+ /* Set all registers and context variables to the predefined values from Easyspin_target_config.h */
+ SetShieldParamsToPredefinedValues();
+
+ /* Disable Easyspin powerstage */
+ for (uint32_t i = 0; i < nbShields; i++) {
+ CmdDisable(i);
+ /* Get Status to clear flags after start up */
+ CmdGetStatus(i);
+ }
}
/******************************************************//**
@@ -123,9 +116,9 @@
* @retval Acceleration in pps^2
**********************************************************/
uint16_t Easyspin::GetAcceleration(uint8_t shieldId)
-{
- return (shieldPrm[shieldId].acceleration);
-}
+{
+ return (shieldPrm[shieldId].acceleration);
+}
/******************************************************//**
* @brief Returns the current speed of the specified shield
@@ -134,7 +127,7 @@
**********************************************************/
uint16_t Easyspin::GetCurrentSpeed(uint8_t shieldId)
{
- return shieldPrm[shieldId].speed;
+ return shieldPrm[shieldId].speed;
}
/******************************************************//**
@@ -143,9 +136,9 @@
* @retval Deceleration in pps^2
**********************************************************/
uint16_t Easyspin::GetDeceleration(uint8_t shieldId)
-{
- return (shieldPrm[shieldId].deceleration);
-}
+{
+ return (shieldPrm[shieldId].deceleration);
+}
/******************************************************//**
* @brief Returns the FW version of the library
@@ -154,17 +147,17 @@
**********************************************************/
uint8_t Easyspin::GetFwVersion(void)
{
- return (Easyspin_FW_VERSION);
+ return (Easyspin_FW_VERSION);
}
/******************************************************//**
* @brief Returns the mark position of the specified shield
* @param[in] shieldId (from 0 to 2)
- * @retval Mark register value converted in a 32b signed integer
+ * @retval Mark register value converted in a 32b signed integer
**********************************************************/
int32_t Easyspin::GetMark(uint8_t shieldId)
{
- return ConvertPosition(CmdGetParam(shieldId,Easyspin_MARK));
+ return ConvertPosition(CmdGetParam(shieldId,Easyspin_MARK));
}
/******************************************************//**
@@ -173,9 +166,9 @@
* @retval maxSpeed in pps
**********************************************************/
uint16_t Easyspin::GetMaxSpeed(uint8_t shieldId)
-{
- return (shieldPrm[shieldId].maxSpeed);
-}
+{
+ return (shieldPrm[shieldId].maxSpeed);
+}
/******************************************************//**
* @brief Returns the min speed of the specified shield
@@ -183,9 +176,9 @@
* @retval minSpeed in pps
**********************************************************/
uint16_t Easyspin::GetMinSpeed(uint8_t shieldId)
-{
- return (shieldPrm[shieldId].minSpeed);
-}
+{
+ return (shieldPrm[shieldId].minSpeed);
+}
/******************************************************//**
* @brief Returns the ABS_POSITION of the specified shield
@@ -194,7 +187,7 @@
**********************************************************/
int32_t Easyspin::GetPosition(uint8_t shieldId)
{
- return ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS));
+ return ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS));
}
/******************************************************//**
@@ -204,7 +197,7 @@
**********************************************************/
shieldState_t Easyspin::GetShieldState(uint8_t shieldId)
{
- return shieldPrm[shieldId].motionState;
+ return shieldPrm[shieldId].motionState;
}
/******************************************************//**
@@ -214,11 +207,11 @@
**********************************************************/
void Easyspin::GoHome(uint8_t shieldId)
{
- GoTo(shieldId, 0);
-}
-
+ GoTo(shieldId, 0);
+}
+
/******************************************************//**
- * @brief Requests the motor to move to the mark position
+ * @brief Requests the motor to move to the mark position
* @param[in] shieldId (from 0 to 2)
* @retval None
**********************************************************/
@@ -227,57 +220,52 @@
uint32_t mark;
mark = ConvertPosition(CmdGetParam(shieldId,Easyspin_MARK));
- GoTo(shieldId,mark);
+ GoTo(shieldId,mark);
}
/******************************************************//**
- * @brief Requests the motor to move to the specified position
+ * @brief Requests the motor to move to the specified position
* @param[in] shieldId (from 0 to 2)
* @param[in] targetPosition absolute position in steps
* @retval None
**********************************************************/
void Easyspin::GoTo(uint8_t shieldId, int32_t targetPosition)
{
- dir_t direction;
- int32_t steps;
-
- /* Eventually deactivate motor */
- if (shieldPrm[shieldId].motionState != INACTIVE)
- {
- HardStop(shieldId);
- }
+ dir_t direction;
+ int32_t steps;
+
+ /* Eventually deactivate motor */
+ if (shieldPrm[shieldId].motionState != INACTIVE) {
+ HardStop(shieldId);
+ }
+
+ /* Get current position */
+ shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS));
+
+ /* Compute the number of steps to perform */
+ steps = targetPosition - shieldPrm[shieldId].currentPosition;
+
+ if (steps >= 0) {
+ shieldPrm[shieldId].stepsToTake = steps;
+ direction = FORWARD;
- /* Get current position */
- shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS));
-
- /* Compute the number of steps to perform */
- steps = targetPosition - shieldPrm[shieldId].currentPosition;
-
- if (steps >= 0)
- {
- shieldPrm[shieldId].stepsToTake = steps;
- direction = FORWARD;
-
- }
- else
- {
- shieldPrm[shieldId].stepsToTake = -steps;
- direction = BACKWARD;
- }
-
- if (steps != 0)
- {
-
- shieldPrm[shieldId].commandExecuted = MOVE_CMD;
-
- /* Direction setup */
- SetDirection(shieldId,direction);
+ } else {
+ shieldPrm[shieldId].stepsToTake = -steps;
+ direction = BACKWARD;
+ }
+
+ if (steps != 0) {
+
+ shieldPrm[shieldId].commandExecuted = MOVE_CMD;
- ComputeSpeedProfile(shieldId, shieldPrm[shieldId].stepsToTake);
-
- /* Motor activation */
- StartMovement(shieldId);
- }
+ /* Direction setup */
+ SetDirection(shieldId,direction);
+
+ ComputeSpeedProfile(shieldId, shieldPrm[shieldId].stepsToTake);
+
+ /* Motor activation */
+ StartMovement(shieldId);
+ }
}
/******************************************************//**
@@ -285,22 +273,22 @@
* @param[in] shieldId (from 0 to 2)
* @retval None
**********************************************************/
-void Easyspin::HardStop(uint8_t shieldId)
+void Easyspin::HardStop(uint8_t shieldId)
{
- /* Disable corresponding PWM */
- PwmStop(shieldId);
+ /* Disable corresponding PWM */
+ PwmStop(shieldId);
- /* Disable power stage */
- CmdDisable(shieldId);
+ /* Disable power stage */
+ CmdDisable(shieldId);
- /* Set inactive state */
- shieldPrm[shieldId].motionState = INACTIVE;
- shieldPrm[shieldId].commandExecuted = NO_CMD;
- shieldPrm[shieldId].stepsToTake = MAX_STEPS;
+ /* Set inactive state */
+ shieldPrm[shieldId].motionState = INACTIVE;
+ shieldPrm[shieldId].commandExecuted = NO_CMD;
+ shieldPrm[shieldId].stepsToTake = MAX_STEPS;
#ifdef _DEBUG_Easyspin
- Serial.println("Inactive\n");
-#endif
+ Serial.println("Inactive\n");
+#endif
}
/******************************************************//**
@@ -312,28 +300,63 @@
**********************************************************/
void Easyspin::Move(uint8_t shieldId, dir_t direction, uint32_t stepCount)
{
- /* Eventually deactivate motor */
- if (shieldPrm[shieldId].motionState != INACTIVE)
- {
- HardStop(shieldId);
- }
-
- if (stepCount != 0)
- {
- shieldPrm[shieldId].stepsToTake = stepCount;
-
- shieldPrm[shieldId].commandExecuted = MOVE_CMD;
-
- shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS));
-
- /* Direction setup */
- SetDirection(shieldId,direction);
+ /* Eventually deactivate motor */
+ if (shieldPrm[shieldId].motionState != INACTIVE) {
+ HardStop(shieldId);
+ }
+
+ if (stepCount != 0) {
+ shieldPrm[shieldId].stepsToTake = stepCount;
+
+ shieldPrm[shieldId].commandExecuted = MOVE_CMD;
+
+ shieldPrm[shieldId].currentPosition = ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS));
+
+ /* Direction setup */
+ SetDirection(shieldId,direction);
+
+ ComputeSpeedProfile(shieldId, stepCount);
+
+ /* Motor activation */
+ StartMovement(shieldId);
+ }
+}
+
- ComputeSpeedProfile(shieldId, stepCount);
-
- /* Motor activation */
- StartMovement(shieldId);
- }
+/******************************************************//**
+ * @brief Turn left or right with the specified angle
+ * @param[in] rotation RIGHT or LEFT
+ * @param[in] angle in degrees
+ * @retval None
+ **********************************************************/
+void Easyspin::Turn(rot_t rotation, uint16_t angle)
+{
+ uint16_t nbStep = (10934*angle)/360;
+ if(rotation==RIGHT) {
+ Move(0, FORWARD, nbStep);
+ Move(1, FORWARD, nbStep);
+ } else {
+ Move(0, BACKWARD, nbStep);
+ Move(1, BACKWARD, nbStep);
+ }
+}
+
+/******************************************************//**
+ * @brief Move forward or backward with the specified distance
+ * @param[in] direction FORWARD or BACKWARD
+ * @param[in] distance in cm
+ * @retval None
+ **********************************************************/
+void Easyspin::Move_cm(dir_t direction, uint16_t distance)
+{
+ uint16_t nbStep = (10000 * distance)/61;
+ if(direction==FORWARD) {
+ Move(0, FORWARD, nbStep);
+ Move(1, BACKWARD, nbStep);
+ } else {
+ Move(0, BACKWARD, nbStep);
+ Move(1, FORWARD, nbStep);
+ }
}
/******************************************************//**
@@ -344,19 +367,18 @@
void Easyspin::ResetAllShields(void)
{
uint8_t loop;
-
- for (loop = 0; loop < numberOfShields; loop++)
- {
- /* Stop movement and disable power stage*/
- HardStop(loop);
- }
+
+ for (loop = 0; loop < numberOfShields; loop++) {
+ /* Stop movement and disable power stage*/
+ HardStop(loop);
+ }
Reset();
- WaitUs(20); // Reset pin must be forced low for at least 10us
+ wait_us(20); // Reset pin must be forced low for at least 10us
ReleaseReset();
}
/******************************************************//**
- * @brief Runs the motor. It will accelerate from the min
+ * @brief Runs the motor. It will accelerate from the min
* speed up to the max speed by using the shield acceleration.
* @param[in] shieldId (from 0 to 2)
* @param[in] direction FORWARD or BACKWARD
@@ -364,19 +386,18 @@
**********************************************************/
void Easyspin::Run(uint8_t shieldId, dir_t direction)
{
- /* Eventually deactivate motor */
- if (shieldPrm[shieldId].motionState != INACTIVE)
- {
- HardStop(shieldId);
- }
-
+ /* Eventually deactivate motor */
+ if (shieldPrm[shieldId].motionState != INACTIVE) {
+ HardStop(shieldId);
+ }
+
/* Direction setup */
SetDirection(shieldId,direction);
shieldPrm[shieldId].commandExecuted = RUN_CMD;
/* Motor activation */
- StartMovement(shieldId);
+ StartMovement(shieldId);
}
/******************************************************//**
@@ -384,42 +405,40 @@
* @param[in] shieldId (from 0 to 2)
* @param[in] newAcc New acceleration to apply in pps^2
* @retval true if the command is successfully executed, else false
- * @note The command is not performed is the shield is executing
+ * @note The command is not performed is the shield is executing
* a MOVE or GOTO command (but it can be used during a RUN command)
**********************************************************/
bool Easyspin::SetAcceleration(uint8_t shieldId,uint16_t newAcc)
-{
- bool cmdExecuted = false;
- if ((newAcc != 0)&&
- ((shieldPrm[shieldId].motionState == INACTIVE)||
- (shieldPrm[shieldId].commandExecuted == RUN_CMD)))
- {
- shieldPrm[shieldId].acceleration = newAcc;
- cmdExecuted = true;
- }
- return cmdExecuted;
-}
+{
+ bool cmdExecuted = false;
+ if ((newAcc != 0)&&
+ ((shieldPrm[shieldId].motionState == INACTIVE)||
+ (shieldPrm[shieldId].commandExecuted == RUN_CMD))) {
+ shieldPrm[shieldId].acceleration = newAcc;
+ cmdExecuted = true;
+ }
+ return cmdExecuted;
+}
/******************************************************//**
* @brief Changes the deceleration of the specified shield
* @param[in] shieldId (from 0 to 2)
* @param[in] newDec New deceleration to apply in pps^2
* @retval true if the command is successfully executed, else false
- * @note The command is not performed is the shield is executing
+ * @note The command is not performed is the shield is executing
* a MOVE or GOTO command (but it can be used during a RUN command)
**********************************************************/
bool Easyspin::SetDeceleration(uint8_t shieldId, uint16_t newDec)
-{
- bool cmdExecuted = false;
- if ((newDec != 0)&&
- ((shieldPrm[shieldId].motionState == INACTIVE)||
- (shieldPrm[shieldId].commandExecuted == RUN_CMD)))
- {
- shieldPrm[shieldId].deceleration = newDec;
- cmdExecuted = true;
- }
- return cmdExecuted;
-}
+{
+ bool cmdExecuted = false;
+ if ((newDec != 0)&&
+ ((shieldPrm[shieldId].motionState == INACTIVE)||
+ (shieldPrm[shieldId].commandExecuted == RUN_CMD))) {
+ shieldPrm[shieldId].deceleration = newDec;
+ cmdExecuted = true;
+ }
+ return cmdExecuted;
+}
/******************************************************//**
* @brief Set current position to be the Home position (ABS pos set to 0)
@@ -428,18 +447,18 @@
**********************************************************/
void Easyspin::SetHome(uint8_t shieldId)
{
- CmdSetParam(shieldId, Easyspin_ABS_POS, 0);
+ CmdSetParam(shieldId, Easyspin_ABS_POS, 0);
}
-
+
/******************************************************//**
- * @brief Sets current position to be the Mark position
+ * @brief Sets current position to be the Mark position
* @param[in] shieldId (from 0 to 2)
* @retval None
**********************************************************/
void Easyspin::SetMark(uint8_t shieldId)
{
- uint32_t mark = CmdGetParam(shieldId,Easyspin_ABS_POS);
- CmdSetParam(shieldId,Easyspin_MARK, mark);
+ uint32_t mark = CmdGetParam(shieldId,Easyspin_ABS_POS);
+ CmdSetParam(shieldId,Easyspin_MARK, mark);
}
/******************************************************//**
@@ -447,46 +466,44 @@
* @param[in] shieldId (from 0 to 2)
* @param[in] newMaxSpeed New max speed to apply in pps
* @retval true if the command is successfully executed, else false
- * @note The command is not performed is the shield is executing
+ * @note The command is not performed is the shield is executing
* a MOVE or GOTO command (but it can be used during a RUN command).
**********************************************************/
bool Easyspin::SetMaxSpeed(uint8_t shieldId, uint16_t newMaxSpeed)
-{
- bool cmdExecuted = false;
- if ((newMaxSpeed > Easyspin_MIN_PWM_FREQ)&&
- (newMaxSpeed <= Easyspin_MAX_PWM_FREQ) &&
- (shieldPrm[shieldId].minSpeed <= newMaxSpeed) &&
- ((shieldPrm[shieldId].motionState == INACTIVE)||
- (shieldPrm[shieldId].commandExecuted == RUN_CMD)))
- {
- shieldPrm[shieldId].maxSpeed = newMaxSpeed;
- cmdExecuted = true;
- }
- return cmdExecuted;
-}
+{
+ bool cmdExecuted = false;
+ if ((newMaxSpeed > Easyspin_MIN_PWM_FREQ)&&
+ (newMaxSpeed <= Easyspin_MAX_PWM_FREQ) &&
+ (shieldPrm[shieldId].minSpeed <= newMaxSpeed) &&
+ ((shieldPrm[shieldId].motionState == INACTIVE)||
+ (shieldPrm[shieldId].commandExecuted == RUN_CMD))) {
+ shieldPrm[shieldId].maxSpeed = newMaxSpeed;
+ cmdExecuted = true;
+ }
+ return cmdExecuted;
+}
/******************************************************//**
* @brief Changes the min speed of the specified shield
* @param[in] shieldId (from 0 to 2)
* @param[in] newMinSpeed New min speed to apply in pps
* @retval true if the command is successfully executed, else false
- * @note The command is not performed is the shield is executing
+ * @note The command is not performed is the shield is executing
* a MOVE or GOTO command (but it can be used during a RUN command).
**********************************************************/
bool Easyspin::SetMinSpeed(uint8_t shieldId, uint16_t newMinSpeed)
-{
- bool cmdExecuted = false;
- if ((newMinSpeed >= Easyspin_MIN_PWM_FREQ)&&
- (newMinSpeed < Easyspin_MAX_PWM_FREQ) &&
- (newMinSpeed <= shieldPrm[shieldId].maxSpeed) &&
- ((shieldPrm[shieldId].motionState == INACTIVE)||
- (shieldPrm[shieldId].commandExecuted == RUN_CMD)))
- {
- shieldPrm[shieldId].minSpeed = newMinSpeed;
- cmdExecuted = true;
- }
- return cmdExecuted;
-}
+{
+ bool cmdExecuted = false;
+ if ((newMinSpeed >= Easyspin_MIN_PWM_FREQ)&&
+ (newMinSpeed < Easyspin_MAX_PWM_FREQ) &&
+ (newMinSpeed <= shieldPrm[shieldId].maxSpeed) &&
+ ((shieldPrm[shieldId].motionState == INACTIVE)||
+ (shieldPrm[shieldId].commandExecuted == RUN_CMD))) {
+ shieldPrm[shieldId].minSpeed = newMinSpeed;
+ cmdExecuted = true;
+ }
+ return cmdExecuted;
+}
/******************************************************//**
* @brief Stops the motor by using the shield deceleration
@@ -495,14 +512,13 @@
* @note The command is not performed is the shield is in INACTIVE state.
**********************************************************/
bool Easyspin::SoftStop(uint8_t shieldId)
-{
- bool cmdExecuted = false;
- if (shieldPrm[shieldId].motionState != INACTIVE)
- {
- shieldPrm[shieldId].commandExecuted = SOFT_STOP_CMD;
- cmdExecuted = true;
- }
- return (cmdExecuted);
+{
+ bool cmdExecuted = false;
+ if (shieldPrm[shieldId].motionState != INACTIVE) {
+ shieldPrm[shieldId].commandExecuted = SOFT_STOP_CMD;
+ cmdExecuted = true;
+ }
+ return (cmdExecuted);
}
/******************************************************//**
@@ -511,7 +527,7 @@
* @retval None
**********************************************************/
void Easyspin::WaitWhileActive(uint8_t shieldId)
- {
+{
/* Wait while motor is running */
while (GetShieldState(shieldId) != INACTIVE);
}
@@ -523,7 +539,7 @@
**********************************************************/
void Easyspin::CmdDisable(uint8_t shieldId)
{
- SendCommand(shieldId, Easyspin_DISABLE);
+ SendCommand(shieldId, Easyspin_DISABLE);
}
/******************************************************//**
@@ -533,7 +549,7 @@
**********************************************************/
void Easyspin::CmdEnable(uint8_t shieldId)
{
- SendCommand(shieldId, Easyspin_ENABLE);
+ SendCommand(shieldId, Easyspin_ENABLE);
}
/******************************************************//**
@@ -544,72 +560,70 @@
**********************************************************/
uint32_t Easyspin::CmdGetParam(uint8_t shieldId, Easyspin_Registers_t param)
{
- uint32_t i;
- uint32_t spiRxData;
- uint8_t maxArgumentNbBytes = 0;
- uint8_t spiIndex = numberOfShields - shieldId - 1;
- bool itDisable = false;
-
- do
- {
- spiPreemtionByIsr = false;
- if (itDisable)
- {
- /* re-enable interrupts if disable in previous iteration */
- flag.enable_irq();
- itDisable = false;
- }
-
- for (i = 0; i < numberOfShields; i++)
- {
- spiTxBursts[0][i] = Easyspin_NOP;
- spiTxBursts[1][i] = Easyspin_NOP;
- spiTxBursts[2][i] = Easyspin_NOP;
- spiTxBursts[3][i] = Easyspin_NOP;
- spiRxBursts[1][i] = 0;
- spiRxBursts[2][i] = 0;
- spiRxBursts[3][i] = 0;
+ uint32_t i;
+ uint32_t spiRxData;
+ uint8_t maxArgumentNbBytes = 0;
+ uint8_t spiIndex = numberOfShields - shieldId - 1;
+ bool itDisable = false;
+
+ do {
+ spiPreemtionByIsr = false;
+ if (itDisable) {
+ /* re-enable interrupts if disable in previous iteration */
+ flag.enable_irq();
+ itDisable = false;
+ }
+
+ for (i = 0; i < numberOfShields; i++) {
+ spiTxBursts[0][i] = Easyspin_NOP;
+ spiTxBursts[1][i] = Easyspin_NOP;
+ spiTxBursts[2][i] = Easyspin_NOP;
+ spiTxBursts[3][i] = Easyspin_NOP;
+ spiRxBursts[1][i] = 0;
+ spiRxBursts[2][i] = 0;
+ spiRxBursts[3][i] = 0;
+ }
+ switch (param) {
+ case Easyspin_ABS_POS:
+ ;
+ case Easyspin_MARK:
+ spiTxBursts[0][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param);
+ maxArgumentNbBytes = 3;
+ break;
+ case Easyspin_EL_POS:
+ ;
+ case Easyspin_CONFIG:
+ ;
+ case Easyspin_STATUS:
+ spiTxBursts[1][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param);
+ maxArgumentNbBytes = 2;
+ break;
+ default:
+ spiTxBursts[2][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param);
+ maxArgumentNbBytes = 1;
+ }
+
+ /* Disable interruption before checking */
+ /* pre-emption by ISR and SPI transfers*/
+ flag.disable_irq();
+ itDisable = true;
+ } while (spiPreemtionByIsr); // check pre-emption by ISR
+
+ for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes;
+ i < Easyspin_CMD_ARG_MAX_NB_BYTES;
+ i++) {
+ WriteBytes(&spiTxBursts[i][0],
+ &spiRxBursts[i][0]);
}
- switch (param)
- {
- case Easyspin_ABS_POS: ;
- case Easyspin_MARK:
- spiTxBursts[0][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param);
- maxArgumentNbBytes = 3;
- break;
- case Easyspin_EL_POS: ;
- case Easyspin_CONFIG: ;
- case Easyspin_STATUS:
- spiTxBursts[1][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param);
- maxArgumentNbBytes = 2;
- break;
- default:
- spiTxBursts[2][spiIndex] = ((uint8_t)Easyspin_GET_PARAM )| (param);
- maxArgumentNbBytes = 1;
- }
-
- /* Disable interruption before checking */
- /* pre-emption by ISR and SPI transfers*/
- flag.disable_irq();
- itDisable = true;
- } while (spiPreemtionByIsr); // check pre-emption by ISR
-
- for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes;
- i < Easyspin_CMD_ARG_MAX_NB_BYTES;
- i++)
- {
- WriteBytes(&spiTxBursts[i][0],
- &spiRxBursts[i][0]);
- }
-
- spiRxData = ((uint32_t)spiRxBursts[1][spiIndex] << 16)|
- (spiRxBursts[2][spiIndex] << 8) |
- (spiRxBursts[3][spiIndex]);
-
- /* re-enable interrupts after SPI transfers*/
- flag.enable_irq();
-
- return (spiRxData);
+
+ spiRxData = ((uint32_t)spiRxBursts[1][spiIndex] << 16)|
+ (spiRxBursts[2][spiIndex] << 8) |
+ (spiRxBursts[3][spiIndex]);
+
+ /* re-enable interrupts after SPI transfers*/
+ flag.enable_irq();
+
+ return (spiRxData);
}
/******************************************************//**
@@ -622,47 +636,43 @@
**********************************************************/
uint16_t Easyspin::CmdGetStatus(uint8_t shieldId)
{
- uint32_t i;
- uint16_t status;
- uint8_t spiIndex = numberOfShields - shieldId - 1;
- bool itDisable = false;
-
- do
- {
- spiPreemtionByIsr = false;
- if (itDisable)
- {
- /* re-enable interrupts if disable in previous iteration */
- flag.enable_irq();
- itDisable = false;
- }
+ uint32_t i;
+ uint16_t status;
+ uint8_t spiIndex = numberOfShields - shieldId - 1;
+ bool itDisable = false;
+
+ do {
+ spiPreemtionByIsr = false;
+ if (itDisable) {
+ /* re-enable interrupts if disable in previous iteration */
+ flag.enable_irq();
+ itDisable = false;
+ }
- for (i = 0; i < numberOfShields; i++)
- {
- spiTxBursts[0][i] = Easyspin_NOP;
- spiTxBursts[1][i] = Easyspin_NOP;
- spiTxBursts[2][i] = Easyspin_NOP;
- spiRxBursts[1][i] = 0;
- spiRxBursts[2][i] = 0;
- }
- spiTxBursts[0][spiIndex] = Easyspin_GET_STATUS;
+ for (i = 0; i < numberOfShields; i++) {
+ spiTxBursts[0][i] = Easyspin_NOP;
+ spiTxBursts[1][i] = Easyspin_NOP;
+ spiTxBursts[2][i] = Easyspin_NOP;
+ spiRxBursts[1][i] = 0;
+ spiRxBursts[2][i] = 0;
+ }
+ spiTxBursts[0][spiIndex] = Easyspin_GET_STATUS;
- /* Disable interruption before checking */
- /* pre-emption by ISR and SPI transfers*/
- flag.disable_irq();
- itDisable = true;
- } while (spiPreemtionByIsr); // check pre-emption by ISR
+ /* Disable interruption before checking */
+ /* pre-emption by ISR and SPI transfers*/
+ flag.disable_irq();
+ itDisable = true;
+ } while (spiPreemtionByIsr); // check pre-emption by ISR
- for (i = 0; i < Easyspin_CMD_ARG_NB_BYTES_GET_STATUS + Easyspin_RSP_NB_BYTES_GET_STATUS; i++)
- {
- WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]);
- }
- status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]);
-
- /* re-enable interrupts after SPI transfers*/
- flag.enable_irq();
-
- return (status);
+ for (i = 0; i < Easyspin_CMD_ARG_NB_BYTES_GET_STATUS + Easyspin_RSP_NB_BYTES_GET_STATUS; i++) {
+ WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]);
+ }
+ status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]);
+
+ /* re-enable interrupts after SPI transfers*/
+ flag.enable_irq();
+
+ return (status);
}
/******************************************************//**
@@ -672,7 +682,7 @@
**********************************************************/
void Easyspin::CmdNop(uint8_t shieldId)
{
- SendCommand(shieldId, Easyspin_NOP);
+ SendCommand(shieldId, Easyspin_NOP);
}
/******************************************************//**
@@ -683,77 +693,74 @@
* @retval None
**********************************************************/
void Easyspin::CmdSetParam(uint8_t shieldId,
- Easyspin_Registers_t param,
- uint32_t value)
+ Easyspin_Registers_t param,
+ uint32_t value)
{
- uint32_t i;
- uint8_t maxArgumentNbBytes = 0;
- uint8_t spiIndex = numberOfShields - shieldId - 1;
- bool itDisable = false;
- do
- {
- spiPreemtionByIsr = false;
- if (itDisable)
- {
- /* re-enable interrupts if disable in previous iteration */
- flag.enable_irq();//interrupts();
- itDisable = false;
- }
- for (i = 0; i < numberOfShields; i++)
- {
- spiTxBursts[0][i] = Easyspin_NOP;
- spiTxBursts[1][i] = Easyspin_NOP;
- spiTxBursts[2][i] = Easyspin_NOP;
- spiTxBursts[3][i] = Easyspin_NOP;
+ uint32_t i;
+ uint8_t maxArgumentNbBytes = 0;
+ uint8_t spiIndex = numberOfShields - shieldId - 1;
+ bool itDisable = false;
+ do {
+ spiPreemtionByIsr = false;
+ if (itDisable) {
+ /* re-enable interrupts if disable in previous iteration */
+ flag.enable_irq();//interrupts();
+ itDisable = false;
+ }
+ for (i = 0; i < numberOfShields; i++) {
+ spiTxBursts[0][i] = Easyspin_NOP;
+ spiTxBursts[1][i] = Easyspin_NOP;
+ spiTxBursts[2][i] = Easyspin_NOP;
+ spiTxBursts[3][i] = Easyspin_NOP;
+ }
+ switch (param) {
+ case Easyspin_ABS_POS:
+ ;
+ case Easyspin_MARK:
+ spiTxBursts[0][spiIndex] = param;
+ spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16);
+ spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
+ maxArgumentNbBytes = 3;
+ break;
+ case Easyspin_EL_POS:
+ ;
+ case Easyspin_CONFIG:
+ spiTxBursts[1][spiIndex] = param;
+ spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
+ maxArgumentNbBytes = 2;
+ break;
+ default:
+ spiTxBursts[2][spiIndex] = param;
+ maxArgumentNbBytes = 1;
+ }
+ spiTxBursts[3][spiIndex] = (uint8_t)(value);
+
+ /* Disable interruption before checking */
+ /* pre-emption by ISR and SPI transfers*/
+ flag.disable_irq();
+ itDisable = true;
+ } while (spiPreemtionByIsr); // check pre-emption by ISR
+
+ /* SPI transfer */
+ for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes;
+ i < Easyspin_CMD_ARG_MAX_NB_BYTES;
+ i++) {
+ WriteBytes(&spiTxBursts[i][0],&spiRxBursts[i][0]);
}
- switch (param)
- {
- case Easyspin_ABS_POS: ;
- case Easyspin_MARK:
- spiTxBursts[0][spiIndex] = param;
- spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16);
- spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
- maxArgumentNbBytes = 3;
- break;
- case Easyspin_EL_POS: ;
- case Easyspin_CONFIG:
- spiTxBursts[1][spiIndex] = param;
- spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
- maxArgumentNbBytes = 2;
- break;
- default:
- spiTxBursts[2][spiIndex] = param;
- maxArgumentNbBytes = 1;
- }
- spiTxBursts[3][spiIndex] = (uint8_t)(value);
-
- /* Disable interruption before checking */
- /* pre-emption by ISR and SPI transfers*/
- flag.disable_irq();
- itDisable = true;
- } while (spiPreemtionByIsr); // check pre-emption by ISR
-
- /* SPI transfer */
- for (i = Easyspin_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes;
- i < Easyspin_CMD_ARG_MAX_NB_BYTES;
- i++)
- {
- WriteBytes(&spiTxBursts[i][0],&spiRxBursts[i][0]);
- }
- /* re-enable interrupts after SPI transfers*/
- flag.enable_irq();
+ /* re-enable interrupts after SPI transfers*/
+ flag.enable_irq();
}
/******************************************************//**
* @brief Reads the Status Register value
* @param[in] shieldId (from 0 to 2)
* @retval Status register valued
- * @note The status register flags are not cleared
+ * @note The status register flags are not cleared
* at the difference with CmdGetStatus()
**********************************************************/
uint16_t Easyspin::ReadStatusRegister(uint8_t shieldId)
{
- return (CmdGetParam(shieldId,Easyspin_STATUS));
+ return (CmdGetParam(shieldId,Easyspin_STATUS));
}
/******************************************************//**
@@ -762,8 +769,8 @@
* @retval None
**********************************************************/
void Easyspin::ReleaseReset(void)
-{
- reset = 1;
+{
+ reset = 1;
}
/******************************************************//**
@@ -773,161 +780,71 @@
**********************************************************/
void Easyspin::Reset(void)
{
- reset = 0;
+ reset = 0;
}
/******************************************************//**
- * @brief Set the stepping mode
+ * @brief Set the stepping mode
* @param[in] shieldId (from 0 to 2)
* @param[in] stepMod from full step to 1/16 microstep as specified in enum Easyspin_STEP_SEL_t
* @retval None
**********************************************************/
void Easyspin::SelectStepMode(uint8_t shieldId, Easyspin_STEP_SEL_t stepMod)
{
- uint8_t stepModeRegister;
-
- /* Eventually deactivate motor */
- if (shieldPrm[shieldId].motionState != INACTIVE)
- {
- HardStop(shieldId);
- }
-
- /* Read Step mode register and clear STEP_SEL field */
- stepModeRegister = (uint8_t)(0xF8 & CmdGetParam(shieldId,Easyspin_STEP_MODE)) ;
-
- /* Apply new step mode */
- CmdSetParam(shieldId, Easyspin_STEP_MODE, stepModeRegister | (uint8_t)stepMod);
+ uint8_t stepModeRegister;
+
+ /* Eventually deactivate motor */
+ if (shieldPrm[shieldId].motionState != INACTIVE) {
+ HardStop(shieldId);
+ }
- /* Reset abs pos register */
- SetHome(shieldId);
+ /* Read Step mode register and clear STEP_SEL field */
+ stepModeRegister = (uint8_t)(0xF8 & CmdGetParam(shieldId,Easyspin_STEP_MODE)) ;
+
+ /* Apply new step mode */
+ CmdSetParam(shieldId, Easyspin_STEP_MODE, stepModeRegister | (uint8_t)stepMod);
+
+ /* Reset abs pos register */
+ SetHome(shieldId);
}
/******************************************************//**
- * @brief Specifies the direction
+ * @brief Specifies the direction
* @param[in] shieldId (from 0 to 2)
* @param[in] dir FORWARD or BACKWARD
- * @note The direction change is only applied if the shield
+ * @note The direction change is only applied if the shield
* is in INACTIVE state
* @retval None
**********************************************************/
void Easyspin::SetDirection(uint8_t shieldId, dir_t dir)
{
- if (shieldPrm[shieldId].motionState == INACTIVE)
- {
- shieldPrm[shieldId].direction = dir;
-
- switch (shieldId)
- {
- case 2:
- dir3 = dir;
- break;
- case 1:
- dir2 = dir;
- break;
- case 0:
- dir1 = dir;
- break;
- default:
- ;
+ if (shieldPrm[shieldId].motionState == INACTIVE) {
+ shieldPrm[shieldId].direction = dir;
+
+ switch (shieldId) {
+ case 2:
+ dir3 = dir;
+ break;
+ case 1:
+ dir2 = dir;
+ break;
+ case 0:
+ dir1 = dir;
+ break;
+ default:
+ ;
+ }
}
- }
-}
-
-/******************************************************//**
- * @brief Waits for the specify delay in milliseconds
- * @param[in] msDelay delay in milliseconds
- * @retval None
- * @note Should only be used for 3 shields configuration.
- * Else, prefer the standard Arduino function delay().
- **********************************************************/
-void Easyspin::WaitMs(uint16_t msDelay)
-{
- uint16_t i;
- for (i = 0; i < msDelay ; i++)
- {
- WaitUs(1000);
- }
}
/******************************************************//**
- * @brief Waits for the specify delay in microseconds
- * @param[in] usDelay delay in microseconds
- * @retval None
- * @note Should be only used for 3 shields configuration.
- * Else, prefer the standard Arduino function delayMicroseconds().
- * Besides, this function is a copy of delayMicroseconds inside
- * the Easyspin library to avoid dependencies conflicts
- * (a redefinition of ISR(TIMER0_OVF_vect)).
- **********************************************************/
-void Easyspin::WaitUs(uint16_t usDelay)
-{
- // calling avrlib's delay_us() function with low values (e.g. 1 or
- // 2 microseconds) gives delays longer than desired.
- //delay_us(us);
-#if F_CPU >= 20000000L
- // for the 20 MHz clock on rare Arduino boards
-
- // for a one-microsecond delay, simply wait 2 cycle and return. The overhead
- // of the function call yields a delay of exactly a one microsecond.
- __asm__ __volatile__ (
- "nop" "\n\t"
- "nop"); //just waiting 2 cycle
- if (--usDelay == 0)
- return;
-
- // the following loop takes a 1/5 of a microsecond (4 cycles)
- // per iteration, so execute it five times for each microsecond of
- // delay requested.
- usDelay = (usDelay<<2) + usDelay; // x5 us
-
- // account for the time taken in the preceeding commands.
- usDelay -= 2;
-
-#elif F_CPU >= 16000000L
- // for the 16 MHz clock on most Arduino boards
-
- // for a one-microsecond delay, simply return. the overhead
- // of the function call yields a delay of approximately 1 1/8 us.
- if (--usDelay == 0)
- return;
-
- // the following loop takes a quarter of a microsecond (4 cycles)
- // per iteration, so execute it four times for each microsecond of
- // delay requested.
- usDelay <<= 2;
-
- // account for the time taken in the preceeding commands.
- usDelay -= 2;
-#else
- // for the 8 MHz internal clock on the ATmega168
-
- // for a one- or two-microsecond delay, simply return. the overhead of
- // the function calls takes more than two microseconds. can't just
- // subtract two, since us is unsigned; we'd overflow.
- if (--usDelay == 0)
- return;
- if (--usDelay == 0)
- return;
-
- // the following loop takes half of a microsecond (4 cycles)
- // per iteration, so execute it twice for each microsecond of
- // delay requested.
- usDelay <<= 1;
-
- // partially compensate for the time taken by the preceeding commands.
- // we can't subtract any more than this or we'd overflow w/ small delays.
- usDelay--;
-#endif
-}
-
-/******************************************************//**
* @brief Gets the pointer to the Easyspin instance
* @param None
* @retval Pointer to the instance of Easyspin
**********************************************************/
class Easyspin* Easyspin::GetInstancePtr(void)
{
- return (class Easyspin*)instancePtr;
+ return (class Easyspin*)instancePtr;
}
/******************************************************//**
@@ -938,156 +855,128 @@
**********************************************************/
void Easyspin::StepClockHandler(uint8_t shieldId)
{
- /* Set isr flag */
- isrFlag = true;
-
- /* Incrementation of the relative position */
- shieldPrm[shieldId].relativePos++;
-
- /* Periodically check that estimated position is correct */
- if ((shieldPrm[shieldId].commandExecuted != RUN_CMD) &&
- ((shieldPrm[shieldId].relativePos % 10) == 00))
- {
- uint32_t AbsPos= ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS));
-
- /* Correct estimated position if needed */
- if (AbsPos != 0)
- {
- if ((shieldPrm[shieldId].direction == FORWARD) &&
- (AbsPos != shieldPrm[shieldId].currentPosition + shieldPrm[shieldId].relativePos))
- {
-#ifdef _DEBUG_Easyspin
- snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "F EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition));
- Serial.println(EasyspinStrOut);
-#endif
- shieldPrm[shieldId].relativePos = AbsPos - shieldPrm[shieldId].currentPosition;
-
- }
- else if ((shieldPrm[shieldId].direction == BACKWARD) &&
- (AbsPos != shieldPrm[shieldId].currentPosition - shieldPrm[shieldId].relativePos))
- {
+ /* Set isr flag */
+ isrFlag = true;
+
+ /* Incrementation of the relative position */
+ shieldPrm[shieldId].relativePos++;
+
+ /* Periodically check that estimated position is correct */
+ if ((shieldPrm[shieldId].commandExecuted != RUN_CMD) &&
+ ((shieldPrm[shieldId].relativePos % 10) == 00)) {
+ uint32_t AbsPos= ConvertPosition(CmdGetParam(shieldId,Easyspin_ABS_POS));
+
+ /* Correct estimated position if needed */
+ if (AbsPos != 0) {
+ if ((shieldPrm[shieldId].direction == FORWARD) &&
+ (AbsPos != shieldPrm[shieldId].currentPosition + shieldPrm[shieldId].relativePos)) {
#ifdef _DEBUG_Easyspin
- snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "B EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition));
- Serial.println(EasyspinStrOut);
-#endif
- shieldPrm[shieldId].relativePos = shieldPrm[shieldId].currentPosition - AbsPos;
- }
- }
- }
-
- switch (shieldPrm[shieldId].motionState)
- {
- case ACCELERATING:
- {
- if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)||
- ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&&
- (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].startDecPos)))
- {
- shieldPrm[shieldId].motionState = DECELERATING;
- shieldPrm[shieldId].accu = 0;
+ snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "F EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition));
+ Serial.println(EasyspinStrOut);
+#endif
+ shieldPrm[shieldId].relativePos = AbsPos - shieldPrm[shieldId].currentPosition;
+
+ } else if ((shieldPrm[shieldId].direction == BACKWARD) &&
+ (AbsPos != shieldPrm[shieldId].currentPosition - shieldPrm[shieldId].relativePos)) {
#ifdef _DEBUG_Easyspin
- snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Dec: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos);
- Serial.println(EasyspinStrOut);
-#endif
- }
- else if ((shieldPrm[shieldId].speed >= shieldPrm[shieldId].maxSpeed)||
- ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&&
- (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].endAccPos)))
- {
- shieldPrm[shieldId].motionState = STEADY;
-#ifdef _DEBUG_Easyspin
- snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos);
- Serial.println(EasyspinStrOut);
-#endif
+ snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "B EstPos:%ld RealPos: %ld\n",shieldPrm[shieldId].relativePos,(AbsPos - shieldPrm[shieldId].currentPosition));
+ Serial.println(EasyspinStrOut);
+#endif
+ shieldPrm[shieldId].relativePos = shieldPrm[shieldId].currentPosition - AbsPos;
+ }
}
- else
- {
- bool speedUpdated = false;
- /* Go on accelerating */
- shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].acceleration << 16) / shieldPrm[shieldId].speed;
- while (shieldPrm[shieldId].accu >= (0X10000L))
- {
- shieldPrm[shieldId].accu -= (0X10000L);
- shieldPrm[shieldId].speed +=1;
- speedUpdated = true;
- }
-
- if (speedUpdated)
- {
- ApplySpeed(shieldId, shieldPrm[shieldId].speed);
- }
- }
- break;
- }
- case STEADY:
- {
- if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)||
- ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&&
- (shieldPrm[shieldId].relativePos >= (shieldPrm[shieldId].startDecPos))) ||
- ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&&
- (shieldPrm[shieldId].speed > shieldPrm[shieldId].maxSpeed)))
- {
- shieldPrm[shieldId].motionState = DECELERATING;
- shieldPrm[shieldId].accu = 0;
- }
- else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&&
- (shieldPrm[shieldId].speed < shieldPrm[shieldId].maxSpeed))
- {
- shieldPrm[shieldId].motionState = ACCELERATING;
- shieldPrm[shieldId].accu = 0;
- }
- break;
}
- case DECELERATING:
- {
- if (((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)&&(shieldPrm[shieldId].speed <= shieldPrm[shieldId].minSpeed))||
- ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&&
- (shieldPrm[shieldId].relativePos >= shieldPrm[shieldId].stepsToTake)))
- {
- /* Motion process complete */
- HardStop(shieldId);
+
+ switch (shieldPrm[shieldId].motionState) {
+ case ACCELERATING: {
+ if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)||
+ ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&&
+ (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].startDecPos))) {
+ shieldPrm[shieldId].motionState = DECELERATING;
+ shieldPrm[shieldId].accu = 0;
#ifdef _DEBUG_Easyspin
- snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Stop: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos );
- Serial.println(EasyspinStrOut);
-#endif
- }
- else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&&
- (shieldPrm[shieldId].speed <= shieldPrm[shieldId].maxSpeed))
- {
- shieldPrm[shieldId].motionState = STEADY;
+ snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Dec: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos);
+ Serial.println(EasyspinStrOut);
+#endif
+ } else if ((shieldPrm[shieldId].speed >= shieldPrm[shieldId].maxSpeed)||
+ ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&&
+ (shieldPrm[shieldId].relativePos == shieldPrm[shieldId].endAccPos))) {
+ shieldPrm[shieldId].motionState = STEADY;
#ifdef _DEBUG_Easyspin
- snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos);
- Serial.println(EasyspinStrOut);
-#endif
- }
- else
- {
- /* Go on decelerating */
- if (shieldPrm[shieldId].speed > shieldPrm[shieldId].minSpeed)
- {
- bool speedUpdated = false;
- shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].deceleration << 16) / shieldPrm[shieldId].speed;
- while (shieldPrm[shieldId].accu >= (0X10000L))
- {
- shieldPrm[shieldId].accu -= (0X10000L);
- shieldPrm[shieldId].speed -=1;
- speedUpdated = true;
- }
- if (speedUpdated)
- {
- ApplySpeed(shieldId, shieldPrm[shieldId].speed);
- }
+ snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Acc->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos);
+ Serial.println(EasyspinStrOut);
+#endif
+ } else {
+ bool speedUpdated = false;
+ /* Go on accelerating */
+ shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].acceleration << 16) / shieldPrm[shieldId].speed;
+ while (shieldPrm[shieldId].accu >= (0X10000L)) {
+ shieldPrm[shieldId].accu -= (0X10000L);
+ shieldPrm[shieldId].speed +=1;
+ speedUpdated = true;
+ }
+
+ if (speedUpdated) {
+ ApplySpeed(shieldId, shieldPrm[shieldId].speed);
+ }
+ }
+ break;
+ }
+ case STEADY: {
+ if ((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)||
+ ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&&
+ (shieldPrm[shieldId].relativePos >= (shieldPrm[shieldId].startDecPos))) ||
+ ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&&
+ (shieldPrm[shieldId].speed > shieldPrm[shieldId].maxSpeed))) {
+ shieldPrm[shieldId].motionState = DECELERATING;
+ shieldPrm[shieldId].accu = 0;
+ } else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&&
+ (shieldPrm[shieldId].speed < shieldPrm[shieldId].maxSpeed)) {
+ shieldPrm[shieldId].motionState = ACCELERATING;
+ shieldPrm[shieldId].accu = 0;
+ }
+ break;
}
- }
- break;
+ case DECELERATING: {
+ if (((shieldPrm[shieldId].commandExecuted == SOFT_STOP_CMD)&&(shieldPrm[shieldId].speed <= shieldPrm[shieldId].minSpeed))||
+ ((shieldPrm[shieldId].commandExecuted != RUN_CMD)&&
+ (shieldPrm[shieldId].relativePos >= shieldPrm[shieldId].stepsToTake))) {
+ /* Motion process complete */
+ HardStop(shieldId);
+#ifdef _DEBUG_Easyspin
+ snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Stop: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos );
+ Serial.println(EasyspinStrOut);
+#endif
+ } else if ((shieldPrm[shieldId].commandExecuted == RUN_CMD)&&
+ (shieldPrm[shieldId].speed <= shieldPrm[shieldId].maxSpeed)) {
+ shieldPrm[shieldId].motionState = STEADY;
+#ifdef _DEBUG_Easyspin
+ snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Dec->Steady: speed: %u relativepos: %ld \n",shieldPrm[shieldId].speed,shieldPrm[shieldId].relativePos);
+ Serial.println(EasyspinStrOut);
+#endif
+ } else {
+ /* Go on decelerating */
+ if (shieldPrm[shieldId].speed > shieldPrm[shieldId].minSpeed) {
+ bool speedUpdated = false;
+ shieldPrm[shieldId].accu += ((uint32_t)shieldPrm[shieldId].deceleration << 16) / shieldPrm[shieldId].speed;
+ while (shieldPrm[shieldId].accu >= (0X10000L)) {
+ shieldPrm[shieldId].accu -= (0X10000L);
+ shieldPrm[shieldId].speed -=1;
+ speedUpdated = true;
+ }
+ if (speedUpdated) {
+ ApplySpeed(shieldId, shieldPrm[shieldId].speed);
+ }
+ }
+ }
+ break;
+ }
+ default: {
+ break;
+ }
}
- default:
- {
- break;
- }
- }
- /* Set isr flag */
- isrFlag = false;
+ /* Set isr flag */
+ isrFlag = false;
}
/******************************************************//**
@@ -1098,31 +987,28 @@
**********************************************************/
void Easyspin::ApplySpeed(uint8_t shieldId, uint16_t newSpeed)
{
- if (newSpeed < Easyspin_MIN_PWM_FREQ)
- {
- newSpeed = Easyspin_MIN_PWM_FREQ;
- }
- if (newSpeed > Easyspin_MAX_PWM_FREQ)
- {
- newSpeed = Easyspin_MAX_PWM_FREQ;
- }
-
- shieldPrm[shieldId].speed = newSpeed;
+ if (newSpeed < Easyspin_MIN_PWM_FREQ) {
+ newSpeed = Easyspin_MIN_PWM_FREQ;
+ }
+ if (newSpeed > Easyspin_MAX_PWM_FREQ) {
+ newSpeed = Easyspin_MAX_PWM_FREQ;
+ }
+
+ shieldPrm[shieldId].speed = newSpeed;
- switch (shieldId)
- {
- case 0:
- Pwm1SetFreq(newSpeed);
- break;
- case 1:
- Pwm2SetFreq(newSpeed);
- break;
- case 2:
- Pwm3SetFreq(newSpeed);
- break;
- default:
- break; //ignore error
- }
+ switch (shieldId) {
+ case 0:
+ Pwm1SetFreq(newSpeed);
+ break;
+ case 1:
+ Pwm2SetFreq(newSpeed);
+ break;
+ case 2:
+ Pwm3SetFreq(newSpeed);
+ break;
+ default:
+ break; //ignore error
+ }
}
/******************************************************//**
@@ -1141,72 +1027,62 @@
**********************************************************/
void Easyspin::ComputeSpeedProfile(uint8_t shieldId, uint32_t nbSteps)
{
- uint32_t reqAccSteps;
+ uint32_t reqAccSteps;
uint32_t reqDecSteps;
-
- /* compute the number of steps to get the targeted speed */
- reqAccSteps = (shieldPrm[shieldId].maxSpeed - shieldPrm[shieldId].minSpeed);
- reqAccSteps *= (shieldPrm[shieldId].maxSpeed + shieldPrm[shieldId].minSpeed);
- reqDecSteps = reqAccSteps;
- reqAccSteps /= (uint32_t)shieldPrm[shieldId].acceleration;
- reqAccSteps /= 2;
+
+ /* compute the number of steps to get the targeted speed */
+ reqAccSteps = (shieldPrm[shieldId].maxSpeed - shieldPrm[shieldId].minSpeed);
+ reqAccSteps *= (shieldPrm[shieldId].maxSpeed + shieldPrm[shieldId].minSpeed);
+ reqDecSteps = reqAccSteps;
+ reqAccSteps /= (uint32_t)shieldPrm[shieldId].acceleration;
+ reqAccSteps /= 2;
- /* compute the number of steps to stop */
- reqDecSteps /= (uint32_t)shieldPrm[shieldId].deceleration;
- reqDecSteps /= 2;
+ /* compute the number of steps to stop */
+ reqDecSteps /= (uint32_t)shieldPrm[shieldId].deceleration;
+ reqDecSteps /= 2;
+
+ if(( reqAccSteps + reqDecSteps ) > nbSteps) {
+ /* Triangular move */
+ /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
- if(( reqAccSteps + reqDecSteps ) > nbSteps)
- {
- /* Triangular move */
- /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
-
- reqDecSteps = ((uint32_t) shieldPrm[shieldId].deceleration * nbSteps) / (shieldPrm[shieldId].acceleration + shieldPrm[shieldId].deceleration);
- if (reqDecSteps > 1)
- {
- reqAccSteps = reqDecSteps - 1;
- if(reqAccSteps == 0)
- {
- reqAccSteps = 1;
- }
- }
- else
- {
- reqAccSteps = 0;
- }
- shieldPrm[shieldId].endAccPos = reqAccSteps;
- shieldPrm[shieldId].startDecPos = reqDecSteps;
- }
- else
- {
- /* Trapezoidal move */
- /* accelerating phase to endAccPos */
- /* steady phase from endAccPos to startDecPos */
- /* decelerating from startDecPos to stepsToTake*/
- shieldPrm[shieldId].endAccPos = reqAccSteps;
- shieldPrm[shieldId].startDecPos = nbSteps - reqDecSteps - 1;
+ reqDecSteps = ((uint32_t) shieldPrm[shieldId].deceleration * nbSteps) / (shieldPrm[shieldId].acceleration + shieldPrm[shieldId].deceleration);
+ if (reqDecSteps > 1) {
+ reqAccSteps = reqDecSteps - 1;
+ if(reqAccSteps == 0) {
+ reqAccSteps = 1;
+ }
+ } else {
+ reqAccSteps = 0;
+ }
+ shieldPrm[shieldId].endAccPos = reqAccSteps;
+ shieldPrm[shieldId].startDecPos = reqDecSteps;
+ } else {
+ /* Trapezoidal move */
+ /* accelerating phase to endAccPos */
+ /* steady phase from endAccPos to startDecPos */
+ /* decelerating from startDecPos to stepsToTake*/
+ shieldPrm[shieldId].endAccPos = reqAccSteps;
+ shieldPrm[shieldId].startDecPos = nbSteps - reqDecSteps - 1;
}
}
/******************************************************//**
* @brief Converts the ABS_POSITION register value to a 32b signed integer
* @param[in] abs_position_reg value of the ABS_POSITION register
- * @retval operation_result 32b signed integer corresponding to the absolute position
+ * @retval operation_result 32b signed integer corresponding to the absolute position
**********************************************************/
int32_t Easyspin::ConvertPosition(uint32_t abs_position_reg)
{
int32_t operation_result;
- if (abs_position_reg & Easyspin_ABS_POS_SIGN_BIT_MASK)
- {
+ if (abs_position_reg & Easyspin_ABS_POS_SIGN_BIT_MASK) {
/* Negative register value */
abs_position_reg = ~abs_position_reg;
abs_position_reg += 1;
operation_result = (int32_t) (abs_position_reg & Easyspin_ABS_POS_VALUE_MASK);
operation_result = -operation_result;
- }
- else
- {
+ } else {
operation_result = (int32_t) abs_position_reg;
}
return operation_result;
@@ -1219,270 +1095,295 @@
**********************************************************/
void Easyspin::FlagInterruptHandler(void)
{
- if (flagInterruptCallback != NULL)
- {
- /* Set isr flag */
- isrFlag = true;
-
- flagInterruptCallback();
-
- /* Reset isr flag */
- isrFlag = false;
- }
+ if (flagInterruptCallback != NULL) {
+ /* Set isr flag */
+ isrFlag = true;
+
+ flagInterruptCallback();
+
+ /* Reset isr flag */
+ isrFlag = false;
+ }
}
/******************************************************//**
* @brief Sends a command without arguments to the Easyspin via the SPI
* @param[in] shieldId (from 0 to 2)
- * @param[in] param Command to send
+ * @param[in] param Command to send
* @retval None
**********************************************************/
void Easyspin::SendCommand(uint8_t shieldId, uint8_t param)
{
- uint8_t spiIndex = numberOfShields - shieldId - 1;
- bool itDisable = false;
-
- do
- {
- spiPreemtionByIsr = false;
- if (itDisable)
- {
- /* re-enable interrupts if disable in previous iteration */
- flag.enable_irq();
- itDisable = false;
- }
-
- for (uint32_t i = 0; i < numberOfShields; i++)
- {
- spiTxBursts[3][i] = Easyspin_NOP;
- }
- spiTxBursts[3][spiIndex] = param;
-
- /* Disable interruption before checking */
- /* pre-emption by ISR and SPI transfers*/
- flag.disable_irq();
- itDisable = true;
- } while (spiPreemtionByIsr); // check pre-emption by ISR
+ uint8_t spiIndex = numberOfShields - shieldId - 1;
+ bool itDisable = false;
+
+ do {
+ spiPreemtionByIsr = false;
+ if (itDisable) {
+ /* re-enable interrupts if disable in previous iteration */
+ flag.enable_irq();
+ itDisable = false;
+ }
- WriteBytes(&spiTxBursts[3][0], &spiRxBursts[3][0]);
-
- /* re-enable interrupts after SPI transfers*/
- flag.enable_irq();
+ for (uint32_t i = 0; i < numberOfShields; i++) {
+ spiTxBursts[3][i] = Easyspin_NOP;
+ }
+ spiTxBursts[3][spiIndex] = param;
+
+ /* Disable interruption before checking */
+ /* pre-emption by ISR and SPI transfers*/
+ flag.disable_irq();
+ itDisable = true;
+ } while (spiPreemtionByIsr); // check pre-emption by ISR
+
+ WriteBytes(&spiTxBursts[3][0], &spiRxBursts[3][0]);
+
+ /* re-enable interrupts after SPI transfers*/
+ flag.enable_irq();
}
/******************************************************//**
- * @brief Sets the registers of the Easyspin to their predefined values
+ * @brief Sets the registers of the Easyspin to their predefined values
* from Easyspin_target_config.h
* @param[in] shieldId (from 0 to 2)
* @retval None
**********************************************************/
void Easyspin::SetRegisterToPredefinedValues(uint8_t shieldId)
{
- CmdSetParam(shieldId,
- Easyspin_ABS_POS,
- 0);
- CmdSetParam(shieldId,
- Easyspin_EL_POS,
- 0);
- CmdSetParam(shieldId,
- Easyspin_MARK,
- 0);
- switch (shieldId)
- {
- case 0:
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
+ Easyspin_ABS_POS,
+ 0);
+ CmdSetParam(shieldId,
+ Easyspin_EL_POS,
+ 0);
+ CmdSetParam(shieldId,
+ Easyspin_MARK,
+ 0);
+ switch (shieldId) {
+ case 0:
+ CmdSetParam(shieldId,
Easyspin_TVAL,
Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_0));
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_T_FAST,
(uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_0 |
(uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_0);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_TON_MIN,
Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_0));
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_TOFF_MIN,
Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_0));
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_OCD_TH,
Easyspin_CONF_PARAM_OCD_TH_SHIELD_0);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_STEP_MODE,
(uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_0 |
(uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_0);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_ALARM_EN,
Easyspin_CONF_PARAM_ALARM_EN_SHIELD_0);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_CONFIG,
(uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_0 |
(uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_0 |
(uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_0 |
(uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_0 |
(uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_0);
- break;
- case 1:
- CmdSetParam(shieldId,
+ break;
+ case 1:
+ CmdSetParam(shieldId,
Easyspin_TVAL,
Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_1));
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_T_FAST,
(uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_1 |
(uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_1);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_TON_MIN,
Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_1));
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_TOFF_MIN,
Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_1));
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_OCD_TH,
Easyspin_CONF_PARAM_OCD_TH_SHIELD_1);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_STEP_MODE,
(uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_1 |
(uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_1);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_ALARM_EN,
Easyspin_CONF_PARAM_ALARM_EN_SHIELD_1);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_CONFIG,
(uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_1 |
(uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_1 |
(uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_1 |
(uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_1 |
(uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_1);
- break;
- case 2:
- CmdSetParam(shieldId,
+ break;
+ case 2:
+ CmdSetParam(shieldId,
Easyspin_TVAL,
Tval_Current_to_Par(Easyspin_CONF_PARAM_TVAL_SHIELD_2));
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_T_FAST,
(uint8_t)Easyspin_CONF_PARAM_TOFF_FAST_SHIELD_2 |
(uint8_t)Easyspin_CONF_PARAM_FAST_STEP_SHIELD_2);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_TON_MIN,
Tmin_Time_to_Par(Easyspin_CONF_PARAM_TON_MIN_SHIELD_2));
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_TOFF_MIN,
Tmin_Time_to_Par(Easyspin_CONF_PARAM_TOFF_MIN_SHIELD_2));
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_OCD_TH,
Easyspin_CONF_PARAM_OCD_TH_SHIELD_2);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_STEP_MODE,
(uint8_t)Easyspin_CONF_PARAM_STEP_SEL_SHIELD_2 |
(uint8_t)Easyspin_CONF_PARAM_SYNC_SEL_SHIELD_2);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_ALARM_EN,
Easyspin_CONF_PARAM_ALARM_EN_SHIELD_2);
- CmdSetParam(shieldId,
+ CmdSetParam(shieldId,
Easyspin_CONFIG,
(uint16_t)Easyspin_CONF_PARAM_CLOCK_SETTING_SHIELD_2 |
(uint16_t)Easyspin_CONF_PARAM_TQ_REG_SHIELD_2 |
(uint16_t)Easyspin_CONF_PARAM_OC_SD_SHIELD_2 |
(uint16_t)Easyspin_CONF_PARAM_SR_SHIELD_2 |
(uint16_t)Easyspin_CONF_PARAM_TOFF_SHIELD_2);
- break;
- default: ;
- }
+ break;
+ default:
+ ;
+ }
}
/******************************************************//**
- * @brief Sets the registers of the Easyspin to their predefined values
+ * @brief Sets the registers of the Easyspin to their predefined values
* from Easyspin_target_config.h
* @param[in] shieldId (from 0 to 2)
* @retval None
**********************************************************/
void Easyspin::WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte)
{
- CS = 0;
- for (uint32_t i = 0; i < numberOfShields; i++)
- {
- *pReceivedByte = spi.write(*pByteToTransmit);
- pByteToTransmit++;
- pReceivedByte++;
- }
- CS = 1;
- if (isrFlag)
- {
- spiPreemtionByIsr = true;
- }
+ CS = 0;
+ for (uint32_t i = 0; i < numberOfShields; i++) {
+ *pReceivedByte = spi.write(*pByteToTransmit);
+ pByteToTransmit++;
+ pReceivedByte++;
+ }
+ CS = 1;
+ if (isrFlag) {
+ spiPreemtionByIsr = true;
+ }
}
/******************************************************//**
* @brief Initialises the PWM uses by the specified shield
* @param[in] shieldId (from 0 to 2)
* @retval None
- * @note Shield 0 uses PW1 based on timer 1
+ * @note Shield 0 uses PW1 based on timer 1
* Shield 1 uses PWM 2 based on timer 2
* Shield 2 uses PWM3 based timer 0
**********************************************************/
void Easyspin::PwmInit(uint8_t shieldId)
{
- switch (shieldId)
- {
- case 0:
- /* PWM1 uses timer 1 */
- /* Initialise timer by setting waveform generation mode
- to PWM phase and Frequency correct: mode = 8
- (WGM10 = 0, WGM11 = 0, WGM12 = 0, WGM13 = 1) */
-
- /* Stop timer1 by clearing CSS bits and set WGM13 and WGM12 */
- //TCCR1B = 0x10;
-
- /* Set WGM10 and WGM11 */
- //TCCR1A = 0x00;
-
- /* Disable Timer1 interrupt */
- //TIMSK1 = 0;
- //pwm1.period_us(400);
- //pwm1.pulsewidth_us(200);
-
- break;
- case 1:
- /* PWM2 uses timer 2 */
- /* Initialise timer by setting waveform generation mode
- to PWM phase correct: mode = 5
- (WGM0 = 1, WGM21 = 0, WGM22 = 1) */
-
- /* Stop timer2 by clearing CSS bits and set WGM22 */
- //TCCR2B = 0x08;
-
- /* Set WGM20 and WGM21 */
- //TCCR2A = 0x01;
-
- /* Disable Timer2 interrupt */
- //TIMSK2 = 0;
- //pwm2.period_us(500);
- //pwm2.pulsewidth_us(100);
- break;
+ switch (shieldId) {
+ case 0:
+ /* PWM1 uses timer 1 */
+ /* Initialise timer by setting waveform generation mode
+ to PWM phase and Frequency correct: mode = 8
+ (WGM10 = 0, WGM11 = 0, WGM12 = 0, WGM13 = 1) */
+
+ /* Stop timer1 by clearing CSS bits and set WGM13 and WGM12 */
+ //TCCR1B = 0x10;
+
+ /* Set WGM10 and WGM11 */
+ //TCCR1A = 0x00;
+
+ /* Disable Timer1 interrupt */
+ //TIMSK1 = 0;
+ //pwm1.period_us(400);
+ //pwm1.pulsewidth_us(200);
+
+ break;
+ case 1:
+ /* PWM2 uses timer 2 */
+ /* Initialise timer by setting waveform generation mode
+ to PWM phase correct: mode = 5
+ (WGM0 = 1, WGM21 = 0, WGM22 = 1) */
+
+ /* Stop timer2 by clearing CSS bits and set WGM22 */
+ //TCCR2B = 0x08;
+
+ /* Set WGM20 and WGM21 */
+ //TCCR2A = 0x01;
+
+ /* Disable Timer2 interrupt */
+ //TIMSK2 = 0;
+ //pwm2.period_us(500);
+ //pwm2.pulsewidth_us(100);
+ break;
- case 2:
- /* PWM3 uses timer 0 */
- /* !!!!! Caution: Calling this configuration will break */
- /* all default Arduino's timing functions as delay(),millis()... */
-
- /* Initialise timer by setting waveform generation mode
- to PWM phase correct: mode = 5
- (WGM0 = 1, WGM21 = 0, WGM22 = 1) */
-
- /* Stop timer0 by clearing CSS bits and set WGM22 */
- //TCCR0B = 0x08;
-
- /* Set WGM00 and WGM01 */
- //TCCR0A = 0x01;
-
- /* Disable Timer0 interrupt */
- //TIMSK0 = 0;
- //pwm3.period_ms(10);
- //pwm3.pulsewidth_ms(1);
- break;
- default:
- break;//ignore error
- }
+ case 2:
+ /* PWM3 uses timer 0 */
+ /* !!!!! Caution: Calling this configuration will break */
+ /* all default Arduino's timing functions as delay(),millis()... */
+
+ /* Initialise timer by setting waveform generation mode
+ to PWM phase correct: mode = 5
+ (WGM0 = 1, WGM21 = 0, WGM22 = 1) */
+
+ /* Stop timer0 by clearing CSS bits and set WGM22 */
+ //TCCR0B = 0x08;
+
+ /* Set WGM00 and WGM01 */
+ //TCCR0A = 0x01;
+
+ /* Disable Timer0 interrupt */
+ //TIMSK0 = 0;
+ //pwm3.period_ms(10);
+ //pwm3.pulsewidth_ms(1);
+ break;
+ default:
+ break;//ignore error
+ }
+}
+
+/******************************************************//**
+ * @brief Ticker1
+ * @param[in]
+ * @retval
+ **********************************************************/
+void Easyspin::tick1()
+{
+ class Easyspin* instancePtr = Easyspin::GetInstancePtr();
+ pwm1 = !pwm1;
+ if (instancePtr != NULL) {
+ if (instancePtr->GetShieldState(0) != INACTIVE) {
+ instancePtr->StepClockHandler(0);
+ }
+ }
+}
+
+/******************************************************//**
+ * @brief Ticker2
+ * @param[in]
+ * @retval
+ **********************************************************/
+void Easyspin::tick2()
+{
+ class Easyspin* instancePtr = Easyspin::GetInstancePtr();
+ pwm2 = !pwm2;
+ if (instancePtr != NULL) {
+ if (instancePtr->GetShieldState(1) != INACTIVE) {
+ instancePtr->StepClockHandler(1);
+ }
+ }
}
/******************************************************//**
@@ -1493,8 +1394,9 @@
**********************************************************/
void Easyspin::Pwm1SetFreq(uint16_t newFreq)
{
- pwm1.period_us(newFreq);
- pwm1.pulsewidth_us(newFreq/2);
+ uint16_t us;
+ us = (1000000 / (newFreq * 2));
+ ticker1.attach_us(this, &Easyspin::tick1, us);
}
/******************************************************//**
@@ -1505,8 +1407,9 @@
**********************************************************/
void Easyspin::Pwm2SetFreq(uint16_t newFreq)
{
- pwm2.period_us(newFreq);
- pwm2.pulsewidth_us(newFreq/2);
+ uint16_t us;
+ us = (1000000 / (newFreq * 2));
+ ticker2.attach_us(this, &Easyspin::tick2, us);
}
/******************************************************//**
@@ -1517,8 +1420,7 @@
**********************************************************/
void Easyspin::Pwm3SetFreq(uint16_t newFreq)
{
- pwm3.period_us(newFreq);
- pwm3.pulsewidth_us(newFreq/2);
+
}
/******************************************************//**
@@ -1528,22 +1430,19 @@
**********************************************************/
void Easyspin::PwmStop(uint8_t shieldId)
{
- switch (shieldId)
- {
- case 0:
- pwm1.period_us(0);
- pwm1.pulsewidth_us(0);
- break;
- case 1:
- pwm2.period_us(0);
- pwm2.pulsewidth_us(0);
- break;
- case 2:
- pwm3.write(0.0f);
- break;
- default:
- break;//ignore error
- }
+ switch (shieldId) {
+ case 0:
+ ticker1.detach();
+ break;
+ case 1:
+ ticker2.detach();
+ break;
+ case 2:
+ ticker3.detach();
+ break;
+ default:
+ break;//ignore error
+ }
}
/******************************************************//**
@@ -1554,25 +1453,24 @@
**********************************************************/
void Easyspin::SetShieldParamsToPredefinedValues(void)
{
- shieldPrm[0].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_0;
- shieldPrm[0].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_0;
- shieldPrm[0].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_0;
- shieldPrm[0].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_0;
-
- shieldPrm[1].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_1;
- shieldPrm[1].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_1;
- shieldPrm[1].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_1;
- shieldPrm[1].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_1;
-
- shieldPrm[2].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_2;
- shieldPrm[2].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_2;
- shieldPrm[2].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_2;
- shieldPrm[2].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_2;
-
- for (uint8_t i = 0; i < numberOfShields; i++)
- {
- SetRegisterToPredefinedValues(i);
- }
+ shieldPrm[0].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_0;
+ shieldPrm[0].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_0;
+ shieldPrm[0].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_0;
+ shieldPrm[0].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_0;
+
+ shieldPrm[1].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_1;
+ shieldPrm[1].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_1;
+ shieldPrm[1].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_1;
+ shieldPrm[1].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_1;
+
+ shieldPrm[2].acceleration = Easyspin_CONF_PARAM_ACC_SHIELD_2;
+ shieldPrm[2].deceleration = Easyspin_CONF_PARAM_DEC_SHIELD_2;
+ shieldPrm[2].maxSpeed = Easyspin_CONF_PARAM_MAX_SPEED_SHIELD_2;
+ shieldPrm[2].minSpeed = Easyspin_CONF_PARAM_MIN_SPEED_SHIELD_2;
+
+ for (uint8_t i = 0; i < numberOfShields; i++) {
+ SetRegisterToPredefinedValues(i);
+ }
}
/******************************************************//**
@@ -1581,62 +1479,43 @@
* @param[in] shieldId (from 0 to 2)
* @retval None
**********************************************************/
-void Easyspin::StartMovement(uint8_t shieldId)
+void Easyspin::StartMovement(uint8_t shieldId)
{
- /* Enable Easyspin powerstage */
- CmdEnable(shieldId);
+ /* Enable Easyspin powerstage */
+ CmdEnable(shieldId);
- if (shieldPrm[shieldId].endAccPos != 0)
- {
- shieldPrm[shieldId].motionState = ACCELERATING;;
- }
- else
- {
- shieldPrm[shieldId].motionState = DECELERATING;
- }
+ if (shieldPrm[shieldId].endAccPos != 0) {
+ shieldPrm[shieldId].motionState = ACCELERATING;;
+ } else {
+ shieldPrm[shieldId].motionState = DECELERATING;
+ }
- shieldPrm[shieldId].accu = 0;
- shieldPrm[shieldId].relativePos = 0;
- ApplySpeed(shieldId, shieldPrm[shieldId].minSpeed);
+ shieldPrm[shieldId].accu = 0;
+ shieldPrm[shieldId].relativePos = 0;
+ ApplySpeed(shieldId, shieldPrm[shieldId].minSpeed);
#ifdef _DEBUG_Easyspin
- snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Stop->Acc: speed: %u relPos: %ld\n", shieldPrm[shieldId].minSpeed, shieldPrm[shieldId].relativePos) ;
- Serial.println(EasyspinStrOut);
-#endif
+ snprintf(EasyspinStrOut, DEBUG_BUFFER_SIZE, "Stop->Acc: speed: %u relPos: %ld\n", shieldPrm[shieldId].minSpeed, shieldPrm[shieldId].relativePos) ;
+ Serial.println(EasyspinStrOut);
+#endif
}
/******************************************************//**
- * @brief Converts mA in compatible values for TVAL register
+ * @brief Converts mA in compatible values for TVAL register
* @param[in] Tval
* @retval TVAL values
**********************************************************/
inline uint8_t Easyspin::Tval_Current_to_Par(double Tval)
{
- return ((uint8_t)(((Tval - 31.25)/31.25)+0.5));
+ return ((uint8_t)(((Tval - 31.25)/31.25)+0.5));
}
/******************************************************//**
- * @brief Convert time in us in compatible values
+ * @brief Convert time in us in compatible values
* for TON_MIN register
* @param[in] Tmin
* @retval TON_MIN values
**********************************************************/
inline uint8_t Easyspin::Tmin_Time_to_Par(double Tmin)
{
- return ((uint8_t)(((Tmin - 0.5)*2)+0.5));
+ return ((uint8_t)(((Tmin - 0.5)*2)+0.5));
}
-
-
-
-/******************************************************//**
- * @brief Debug function to get the amount of free ram
- * @param None
- * @retval number of bytes of free ram
- **********************************************************/
-#ifdef _DEBUG_Easyspin
-uint16_t GetFreeRam (void)
-{
- extern uint16_t __heap_start, *__brkval;
- uint16_t v;
- return (uint16_t) &v - (__brkval == 0 ? (uint16_t) &__heap_start : (uint16_t) __brkval);
-}
-#endif