X_NUCLEO_IHM01A1_Disabled_Control - Personal fork of the library for direct control i…

Users » Arkadi » Code » X_NUCLEO_IHM01A1_Disabled_Control

Arkadi Rafalovich / X_NUCLEO_IHM01A1_Disabled_Control

Personal fork of the library for direct control instead of library control

Dependencies: X_NUCLEO_COMMON

Dependents: Thesis_Rotating_Platform

Fork of X_NUCLEO_IHM01A1 by Arkadi Rafalovich

Components/l6474/l6474_class.cpp@18:2d6ab2b93685, 2016-01-13 (annotated)

Committer:: Davidroid
Date:: Wed Jan 13 14:35:59 2016 +0000
Revision:: 18:2d6ab2b93685
Parent:: 15:40470df81d9a
Child:: 21:83138e702683

+ Initialization structure added.; + StepperMotor's GetParameters()/SetParameters() methods updated to handle floating-point numbers.; + L6474's GetParameters()/SetParameters() methods updated to re-format parameters.;

Who changed what in which revision?

User	Revision	Line number	New contents of line
Davidroid	0:2887415a46cd	1	/**
Davidroid	0:2887415a46cd	2	******************************************************************************
Davidroid	0:2887415a46cd	3	* @file l6474_class.cpp
Davidroid	0:2887415a46cd	4	* @author IPC Rennes
Davidroid	0:2887415a46cd	5	* @version V1.5.0
Davidroid	0:2887415a46cd	6	* @date November 12, 2014
Davidroid	0:2887415a46cd	7	* @brief L6474 driver (fully integrated microstepping motor driver)
Davidroid	0:2887415a46cd	8	* @note (C) COPYRIGHT 2014 STMicroelectronics
Davidroid	0:2887415a46cd	9	******************************************************************************
Davidroid	0:2887415a46cd	10	* @attention
Davidroid	0:2887415a46cd	11	*
Davidroid	0:2887415a46cd	12	* <h2><center>© COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
Davidroid	0:2887415a46cd	13	*
Davidroid	0:2887415a46cd	14	* Redistribution and use in source and binary forms, with or without modification,
Davidroid	0:2887415a46cd	15	* are permitted provided that the following conditions are met:
Davidroid	0:2887415a46cd	16	* 1. Redistributions of source code must retain the above copyright notice,
Davidroid	0:2887415a46cd	17	* this list of conditions and the following disclaimer.
Davidroid	0:2887415a46cd	18	* 2. Redistributions in binary form must reproduce the above copyright notice,
Davidroid	0:2887415a46cd	19	* this list of conditions and the following disclaimer in the documentation
Davidroid	0:2887415a46cd	20	* and/or other materials provided with the distribution.
Davidroid	0:2887415a46cd	21	* 3. Neither the name of STMicroelectronics nor the names of its contributors
Davidroid	0:2887415a46cd	22	* may be used to endorse or promote products derived from this software
Davidroid	0:2887415a46cd	23	* without specific prior written permission.
Davidroid	0:2887415a46cd	24	*
Davidroid	0:2887415a46cd	25	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Davidroid	0:2887415a46cd	26	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Davidroid	0:2887415a46cd	27	* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Davidroid	0:2887415a46cd	28	* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Davidroid	0:2887415a46cd	29	* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Davidroid	0:2887415a46cd	30	* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Davidroid	0:2887415a46cd	31	* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Davidroid	0:2887415a46cd	32	* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Davidroid	0:2887415a46cd	33	* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Davidroid	0:2887415a46cd	34	* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Davidroid	0:2887415a46cd	35	*
Davidroid	0:2887415a46cd	36	******************************************************************************
Davidroid	0:2887415a46cd	37	*/
Davidroid	0:2887415a46cd	38
Davidroid	0:2887415a46cd	39
Davidroid	0:2887415a46cd	40	/* Generated with Stm32CubeTOO -----------------------------------------------*/
Davidroid	0:2887415a46cd	41
Davidroid	0:2887415a46cd	42
Davidroid	0:2887415a46cd	43	/* Revision ------------------------------------------------------------------*/
Davidroid	0:2887415a46cd	44	/*
Davidroid	0:2887415a46cd	45	Repository: http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev
Davidroid	0:2887415a46cd	46	Branch/Trunk/Tag: trunk
Davidroid	0:2887415a46cd	47	Based on: X-CUBE-SPN1/trunk/Drivers/BSP/Components/l6474/l6474.c
Davidroid	14:73823a4344be	48	Revision: 0
Davidroid	0:2887415a46cd	49	*/
Davidroid	0:2887415a46cd	50
Davidroid	0:2887415a46cd	51
Davidroid	0:2887415a46cd	52	/* Includes ------------------------------------------------------------------*/
Davidroid	0:2887415a46cd	53
Davidroid	0:2887415a46cd	54	#include "l6474_class.h"
Davidroid	0:2887415a46cd	55	#include "l6474.h"
Davidroid	0:2887415a46cd	56
Davidroid	1:b38ebb8ea286	57
Davidroid	15:40470df81d9a	58	/* Definitions ---------------------------------------------------------------*/
Davidroid	0:2887415a46cd	59
Davidroid	15:40470df81d9a	60	/* Error while initialising the SPI. */
Davidroid	5:d3c78f12a78d	61	#define L6474_ERROR_0 (0x8000)
Davidroid	15:40470df81d9a	62
Davidroid	15:40470df81d9a	63	/* Error of bad SPI transaction. */
Davidroid	5:d3c78f12a78d	64	#define L6474_ERROR_1 (0x8001)
Davidroid	0:2887415a46cd	65
Davidroid	15:40470df81d9a	66	/* Maximum number of steps. */
Davidroid	5:d3c78f12a78d	67	#define MAX_STEPS (0x7FFFFFFF)
Davidroid	0:2887415a46cd	68
Davidroid	15:40470df81d9a	69	/* Maximum frequency of the PWMs in Hz. */
Davidroid	0:2887415a46cd	70	#define L6474_MAX_PWM_FREQ (10000)
Davidroid	0:2887415a46cd	71
Davidroid	15:40470df81d9a	72	/* Minimum frequency of the PWMs in Hz. */
Davidroid	0:2887415a46cd	73	#define L6474_MIN_PWM_FREQ (2)
Davidroid	0:2887415a46cd	74
Davidroid	0:2887415a46cd	75
Davidroid	15:40470df81d9a	76	/* Variables ----------------------------------------------------------------*/
Davidroid	0:2887415a46cd	77
Davidroid	0:2887415a46cd	78	/* Number of devices. */
Davidroid	18:2d6ab2b93685	79	uint8_t L6474::number_of_devices = 0;
Davidroid	0:2887415a46cd	80
Davidroid	6:a47569fc7534	81	/* ISR flags used to restart an interrupted SPI transfer when an error is reported. */
Davidroid	18:2d6ab2b93685	82	bool L6474::spi_preemtion_by_isr = FALSE;
Davidroid	18:2d6ab2b93685	83	bool L6474::isr_flag = FALSE;
Davidroid	6:a47569fc7534	84
Davidroid	5:d3c78f12a78d	85	/* SPI Transmission for Daisy-Chain Configuration. */
Davidroid	18:2d6ab2b93685	86	uint8_t L6474::spi_tx_bursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES];
Davidroid	18:2d6ab2b93685	87	uint8_t L6474::spi_rx_bursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES];
Davidroid	5:d3c78f12a78d	88
Davidroid	0:2887415a46cd	89
Davidroid	1:b38ebb8ea286	90	/* Methods -------------------------------------------------------------------*/
Davidroid	0:2887415a46cd	91
Davidroid	5:d3c78f12a78d	92	/**********************************************************
Davidroid	0:2887415a46cd	93	* @brief Attaches a user callback to the error Handler.
Davidroid	0:2887415a46cd	94	* The call back will be then called each time the library
Davidroid	0:2887415a46cd	95	* detects an error
Davidroid	0:2887415a46cd	96	* @param[in] callback Name of the callback to attach
Davidroid	0:2887415a46cd	97	* to the error Hanlder
Davidroid	0:2887415a46cd	98	* @retval None
Davidroid	0:2887415a46cd	99	**********************************************************/
Davidroid	0:2887415a46cd	100	void L6474::L6474_AttachErrorHandler(void (*callback)(uint16_t error))
Davidroid	0:2887415a46cd	101	{
Davidroid	18:2d6ab2b93685	102	error_handler_callback = (void (*)(uint16_t error)) callback;
Davidroid	0:2887415a46cd	103	}
Davidroid	0:2887415a46cd	104
Davidroid	5:d3c78f12a78d	105	/**********************************************************
Davidroid	0:2887415a46cd	106	* @brief Starts the L6474 library
Davidroid	18:2d6ab2b93685	107	* @param init Initialization structure.
Davidroid	18:2d6ab2b93685	108	* @retval COMPONENT_OK in case of success.
Davidroid	0:2887415a46cd	109	**********************************************************/
Davidroid	18:2d6ab2b93685	110	DrvStatusTypeDef L6474::L6474_Init(L6474_InitTypeDef *init)
Davidroid	0:2887415a46cd	111	{
Davidroid	0:2887415a46cd	112	/* Initialise the PWMs used for the Step clocks ----------------------------*/
Davidroid	0:2887415a46cd	113	L6474_PwmInit();
Davidroid	0:2887415a46cd	114
Davidroid	0:2887415a46cd	115	/* Initialise the L6474s ------------------------------------------------*/
Davidroid	0:2887415a46cd	116
Davidroid	0:2887415a46cd	117	/* Standby-reset deactivation */
Davidroid	0:2887415a46cd	118	L6474_ReleaseReset();
Davidroid	0:2887415a46cd	119
Davidroid	0:2887415a46cd	120	/* Let a delay after reset */
Davidroid	0:2887415a46cd	121	L6474_Delay(1);
Davidroid	0:2887415a46cd	122
Davidroid	18:2d6ab2b93685	123	/* Set device parameters to the predefined values from "l6474_target_config.h". */
Davidroid	0:2887415a46cd	124	L6474_SetDeviceParamsToPredefinedValues();
Davidroid	0:2887415a46cd	125
Davidroid	18:2d6ab2b93685	126	if (init == NULL)
Davidroid	18:2d6ab2b93685	127	/* Set device registers to the predefined values from "l6474_target_config.h". */
Davidroid	18:2d6ab2b93685	128	L6474_SetRegisterToPredefinedValues();
Davidroid	18:2d6ab2b93685	129	else
Davidroid	18:2d6ab2b93685	130	/* Set device registers to the passed initialization values. */
Davidroid	18:2d6ab2b93685	131	L6474_SetRegisterToInitializationValues(init);
Davidroid	18:2d6ab2b93685	132
Davidroid	0:2887415a46cd	133	/* Disable L6474 powerstage */
Davidroid	0:2887415a46cd	134	L6474_CmdDisable();
Davidroid	0:2887415a46cd	135
Davidroid	0:2887415a46cd	136	/* Get Status to clear flags after start up */
Davidroid	0:2887415a46cd	137	L6474_CmdGetStatus();
Davidroid	0:2887415a46cd	138
Davidroid	0:2887415a46cd	139	return COMPONENT_OK;
Davidroid	0:2887415a46cd	140	}
Davidroid	0:2887415a46cd	141
Davidroid	5:d3c78f12a78d	142	/**********************************************************
Davidroid	0:2887415a46cd	143	* @brief Read id
Davidroid	0:2887415a46cd	144	* @param id pointer to the identifier to be read.
Davidroid	18:2d6ab2b93685	145	* @retval COMPONENT_OK in case of success.
Davidroid	0:2887415a46cd	146	**********************************************************/
Davidroid	0:2887415a46cd	147	DrvStatusTypeDef L6474::L6474_ReadID(uint8_t *id)
Davidroid	0:2887415a46cd	148	{
Davidroid	18:2d6ab2b93685	149	*id = device_instance;
Davidroid	0:2887415a46cd	150
Davidroid	0:2887415a46cd	151	return COMPONENT_OK;
Davidroid	0:2887415a46cd	152	}
Davidroid	0:2887415a46cd	153
Davidroid	5:d3c78f12a78d	154	/**********************************************************
Davidroid	0:2887415a46cd	155	* @brief Returns the acceleration of the specified device
Davidroid	0:2887415a46cd	156	* @retval Acceleration in pps^2
Davidroid	0:2887415a46cd	157	**********************************************************/
Davidroid	0:2887415a46cd	158	uint16_t L6474::L6474_GetAcceleration(void)
Davidroid	0:2887415a46cd	159	{
Davidroid	18:2d6ab2b93685	160	return (device_prm.acceleration);
Davidroid	0:2887415a46cd	161	}
Davidroid	0:2887415a46cd	162
Davidroid	5:d3c78f12a78d	163	/**********************************************************
Davidroid	0:2887415a46cd	164	* @brief Returns the current speed of the specified device
Davidroid	0:2887415a46cd	165	* @retval Speed in pps
Davidroid	0:2887415a46cd	166	**********************************************************/
Davidroid	0:2887415a46cd	167	uint16_t L6474::L6474_GetCurrentSpeed(void)
Davidroid	0:2887415a46cd	168	{
Davidroid	18:2d6ab2b93685	169	return device_prm.speed;
Davidroid	0:2887415a46cd	170	}
Davidroid	0:2887415a46cd	171
Davidroid	5:d3c78f12a78d	172	/**********************************************************
Davidroid	0:2887415a46cd	173	* @brief Returns the deceleration of the specified device
Davidroid	0:2887415a46cd	174	* @retval Deceleration in pps^2
Davidroid	0:2887415a46cd	175	**********************************************************/
Davidroid	0:2887415a46cd	176	uint16_t L6474::L6474_GetDeceleration(void)
Davidroid	0:2887415a46cd	177	{
Davidroid	18:2d6ab2b93685	178	return (device_prm.deceleration);
Davidroid	0:2887415a46cd	179	}
Davidroid	0:2887415a46cd	180
Davidroid	5:d3c78f12a78d	181	/**********************************************************
Davidroid	0:2887415a46cd	182	* @brief Returns the device state
Davidroid	0:2887415a46cd	183	* @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE)
Davidroid	0:2887415a46cd	184	**********************************************************/
Davidroid	0:2887415a46cd	185	motorState_t L6474::L6474_GetDeviceState(void)
Davidroid	0:2887415a46cd	186	{
Davidroid	18:2d6ab2b93685	187	return device_prm.motionState;
Davidroid	0:2887415a46cd	188	}
Davidroid	0:2887415a46cd	189
Davidroid	5:d3c78f12a78d	190	/**********************************************************
Davidroid	0:2887415a46cd	191	* @brief Returns the FW version of the library
Davidroid	0:2887415a46cd	192	* @param None
Davidroid	0:2887415a46cd	193	* @retval L6474_FW_VERSION
Davidroid	0:2887415a46cd	194	**********************************************************/
Davidroid	0:2887415a46cd	195	uint8_t L6474::L6474_GetFwVersion(void)
Davidroid	0:2887415a46cd	196	{
Davidroid	0:2887415a46cd	197	return (L6474_FW_VERSION);
Davidroid	0:2887415a46cd	198	}
Davidroid	0:2887415a46cd	199
Davidroid	5:d3c78f12a78d	200	/**********************************************************
Davidroid	0:2887415a46cd	201	* @brief Returns the mark position of the specified device
Davidroid	0:2887415a46cd	202	* @retval Mark register value converted in a 32b signed integer
Davidroid	0:2887415a46cd	203	**********************************************************/
Davidroid	0:2887415a46cd	204	int32_t L6474::L6474_GetMark(void)
Davidroid	0:2887415a46cd	205	{
Davidroid	0:2887415a46cd	206	return L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK));
Davidroid	0:2887415a46cd	207	}
Davidroid	0:2887415a46cd	208
Davidroid	5:d3c78f12a78d	209	/**********************************************************
Davidroid	0:2887415a46cd	210	* @brief Returns the max speed of the specified device
Davidroid	0:2887415a46cd	211	* @retval maxSpeed in pps
Davidroid	0:2887415a46cd	212	**********************************************************/
Davidroid	0:2887415a46cd	213	uint16_t L6474::L6474_GetMaxSpeed(void)
Davidroid	0:2887415a46cd	214	{
Davidroid	18:2d6ab2b93685	215	return (device_prm.maxSpeed);
Davidroid	0:2887415a46cd	216	}
Davidroid	0:2887415a46cd	217
Davidroid	5:d3c78f12a78d	218	/**********************************************************
Davidroid	0:2887415a46cd	219	* @brief Returns the min speed of the specified device
Davidroid	0:2887415a46cd	220	* @retval minSpeed in pps
Davidroid	0:2887415a46cd	221	**********************************************************/
Davidroid	0:2887415a46cd	222	uint16_t L6474::L6474_GetMinSpeed(void)
Davidroid	0:2887415a46cd	223	{
Davidroid	18:2d6ab2b93685	224	return (device_prm.minSpeed);
Davidroid	0:2887415a46cd	225	}
Davidroid	0:2887415a46cd	226
Davidroid	5:d3c78f12a78d	227	/**********************************************************
Davidroid	0:2887415a46cd	228	* @brief Returns the ABS_POSITION of the specified device
Davidroid	0:2887415a46cd	229	* @retval ABS_POSITION register value converted in a 32b signed integer
Davidroid	0:2887415a46cd	230	**********************************************************/
Davidroid	0:2887415a46cd	231	int32_t L6474::L6474_GetPosition(void)
Davidroid	0:2887415a46cd	232	{
Davidroid	0:2887415a46cd	233	return L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS));
Davidroid	0:2887415a46cd	234	}
Davidroid	0:2887415a46cd	235
Davidroid	5:d3c78f12a78d	236	/**********************************************************
Davidroid	0:2887415a46cd	237	* @brief Requests the motor to move to the home position (ABS_POSITION = 0)
Davidroid	0:2887415a46cd	238	* @retval None
Davidroid	0:2887415a46cd	239	**********************************************************/
Davidroid	0:2887415a46cd	240	void L6474::L6474_GoHome(void)
Davidroid	0:2887415a46cd	241	{
Davidroid	0:2887415a46cd	242	L6474_GoTo(0);
Davidroid	0:2887415a46cd	243	}
Davidroid	0:2887415a46cd	244
Davidroid	5:d3c78f12a78d	245	/**********************************************************
Davidroid	0:2887415a46cd	246	* @brief Requests the motor to move to the mark position
Davidroid	0:2887415a46cd	247	* @retval None
Davidroid	0:2887415a46cd	248	**********************************************************/
Davidroid	0:2887415a46cd	249	void L6474::L6474_GoMark(void)
Davidroid	0:2887415a46cd	250	{
Davidroid	0:2887415a46cd	251	uint32_t mark;
Davidroid	0:2887415a46cd	252
Davidroid	0:2887415a46cd	253	mark = L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK));
Davidroid	0:2887415a46cd	254	L6474_GoTo(mark);
Davidroid	0:2887415a46cd	255	}
Davidroid	0:2887415a46cd	256
Davidroid	5:d3c78f12a78d	257	/**********************************************************
Davidroid	0:2887415a46cd	258	* @brief Requests the motor to move to the specified position
Davidroid	0:2887415a46cd	259	* @param[in] targetPosition absolute position in steps
Davidroid	0:2887415a46cd	260	* @retval None
Davidroid	0:2887415a46cd	261	**********************************************************/
Davidroid	0:2887415a46cd	262	void L6474::L6474_GoTo(int32_t targetPosition)
Davidroid	0:2887415a46cd	263	{
Davidroid	5:d3c78f12a78d	264	motorDir_t direction;
Davidroid	0:2887415a46cd	265	int32_t steps;
Davidroid	0:2887415a46cd	266
Davidroid	0:2887415a46cd	267	/* Eventually deactivate motor */
Davidroid	18:2d6ab2b93685	268	if (device_prm.motionState != INACTIVE)
Davidroid	0:2887415a46cd	269	{
Davidroid	0:2887415a46cd	270	L6474_HardStop();
Davidroid	0:2887415a46cd	271	}
Davidroid	0:2887415a46cd	272
Davidroid	0:2887415a46cd	273	/* Get current position */
Davidroid	18:2d6ab2b93685	274	device_prm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS));
Davidroid	0:2887415a46cd	275
Davidroid	0:2887415a46cd	276	/* Compute the number of steps to perform */
Davidroid	18:2d6ab2b93685	277	steps = targetPosition - device_prm.currentPosition;
Davidroid	0:2887415a46cd	278
Davidroid	0:2887415a46cd	279	if (steps >= 0)
Davidroid	0:2887415a46cd	280	{
Davidroid	18:2d6ab2b93685	281	device_prm.stepsToTake = steps;
Davidroid	5:d3c78f12a78d	282	direction = FORWARD;
Davidroid	0:2887415a46cd	283	}
Davidroid	0:2887415a46cd	284	else
Davidroid	0:2887415a46cd	285	{
Davidroid	18:2d6ab2b93685	286	device_prm.stepsToTake = -steps;
Davidroid	5:d3c78f12a78d	287	direction = BACKWARD;
Davidroid	0:2887415a46cd	288	}
Davidroid	0:2887415a46cd	289
Davidroid	0:2887415a46cd	290	if (steps != 0)
Davidroid	0:2887415a46cd	291	{
Davidroid	18:2d6ab2b93685	292	device_prm.commandExecuted = MOVE_CMD;
Davidroid	0:2887415a46cd	293
Davidroid	0:2887415a46cd	294	/* Direction setup */
Davidroid	0:2887415a46cd	295	L6474_SetDirection(direction);
Davidroid	0:2887415a46cd	296
Davidroid	18:2d6ab2b93685	297	L6474_ComputeSpeedProfile(device_prm.stepsToTake);
Davidroid	0:2887415a46cd	298
Davidroid	0:2887415a46cd	299	/* Motor activation */
Davidroid	0:2887415a46cd	300	L6474_StartMovement();
Davidroid	0:2887415a46cd	301	}
Davidroid	0:2887415a46cd	302	}
Davidroid	0:2887415a46cd	303
Davidroid	5:d3c78f12a78d	304	/**********************************************************
Davidroid	0:2887415a46cd	305	* @brief Immediatly stops the motor and disable the power bridge
Davidroid	0:2887415a46cd	306	* @retval None
Davidroid	0:2887415a46cd	307	**********************************************************/
Davidroid	0:2887415a46cd	308	void L6474::L6474_HardStop(void)
Davidroid	0:2887415a46cd	309	{
Davidroid	0:2887415a46cd	310	/* Disable corresponding PWM */
Davidroid	0:2887415a46cd	311	L6474_PwmStop();
Davidroid	0:2887415a46cd	312
Davidroid	0:2887415a46cd	313	/* Set inactive state */
Davidroid	18:2d6ab2b93685	314	device_prm.motionState = INACTIVE;
Davidroid	18:2d6ab2b93685	315	device_prm.commandExecuted = NO_CMD;
Davidroid	18:2d6ab2b93685	316	device_prm.stepsToTake = MAX_STEPS;
Davidroid	0:2887415a46cd	317	}
Davidroid	0:2887415a46cd	318
Davidroid	5:d3c78f12a78d	319	/**********************************************************
Davidroid	0:2887415a46cd	320	* @brief Moves the motor of the specified number of steps
Davidroid	0:2887415a46cd	321	* @param[in] direction FORWARD or BACKWARD
Davidroid	0:2887415a46cd	322	* @param[in] stepCount Number of steps to perform
Davidroid	0:2887415a46cd	323	* @retval None
Davidroid	0:2887415a46cd	324	**********************************************************/
Davidroid	5:d3c78f12a78d	325	void L6474::L6474_Move(motorDir_t direction, uint32_t stepCount)
Davidroid	0:2887415a46cd	326	{
Davidroid	0:2887415a46cd	327	/* Eventually deactivate motor */
Davidroid	18:2d6ab2b93685	328	if (device_prm.motionState != INACTIVE)
Davidroid	0:2887415a46cd	329	{
Davidroid	0:2887415a46cd	330	L6474_HardStop();
Davidroid	0:2887415a46cd	331	}
Davidroid	0:2887415a46cd	332
Davidroid	0:2887415a46cd	333	if (stepCount != 0)
Davidroid	0:2887415a46cd	334	{
Davidroid	18:2d6ab2b93685	335	device_prm.stepsToTake = stepCount;
Davidroid	0:2887415a46cd	336
Davidroid	18:2d6ab2b93685	337	device_prm.commandExecuted = MOVE_CMD;
Davidroid	0:2887415a46cd	338
Davidroid	18:2d6ab2b93685	339	device_prm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS));
Davidroid	0:2887415a46cd	340
Davidroid	0:2887415a46cd	341	/* Direction setup */
Davidroid	0:2887415a46cd	342	L6474_SetDirection(direction);
Davidroid	0:2887415a46cd	343
Davidroid	0:2887415a46cd	344	L6474_ComputeSpeedProfile(stepCount);
Davidroid	0:2887415a46cd	345
Davidroid	0:2887415a46cd	346	/* Motor activation */
Davidroid	0:2887415a46cd	347	L6474_StartMovement();
Davidroid	0:2887415a46cd	348	}
Davidroid	0:2887415a46cd	349	}
Davidroid	0:2887415a46cd	350
Davidroid	5:d3c78f12a78d	351	/**********************************************************
Davidroid	0:2887415a46cd	352	* @brief Runs the motor. It will accelerate from the min
Davidroid	0:2887415a46cd	353	* speed up to the max speed by using the device acceleration.
Davidroid	0:2887415a46cd	354	* @param[in] direction FORWARD or BACKWARD
Davidroid	0:2887415a46cd	355	* @retval None
Davidroid	0:2887415a46cd	356	**********************************************************/
Davidroid	5:d3c78f12a78d	357	void L6474::L6474_Run(motorDir_t direction)
Davidroid	0:2887415a46cd	358	{
Davidroid	0:2887415a46cd	359	/* Eventually deactivate motor */
Davidroid	18:2d6ab2b93685	360	if (device_prm.motionState != INACTIVE)
Davidroid	0:2887415a46cd	361	{
Davidroid	0:2887415a46cd	362	L6474_HardStop();
Davidroid	0:2887415a46cd	363	}
Davidroid	0:2887415a46cd	364
Davidroid	0:2887415a46cd	365	/* Direction setup */
Davidroid	0:2887415a46cd	366	L6474_SetDirection(direction);
Davidroid	0:2887415a46cd	367
Davidroid	18:2d6ab2b93685	368	device_prm.commandExecuted = RUN_CMD;
Davidroid	0:2887415a46cd	369
Davidroid	0:2887415a46cd	370	/* Motor activation */
Davidroid	0:2887415a46cd	371	L6474_StartMovement();
Davidroid	0:2887415a46cd	372	}
Davidroid	0:2887415a46cd	373
Davidroid	5:d3c78f12a78d	374	/**********************************************************
Davidroid	0:2887415a46cd	375	* @brief Changes the acceleration of the specified device
Davidroid	0:2887415a46cd	376	* @param[in] newAcc New acceleration to apply in pps^2
Davidroid	0:2887415a46cd	377	* @retval true if the command is successfully executed, else false
Davidroid	0:2887415a46cd	378	* @note The command is not performed is the device is executing
Davidroid	0:2887415a46cd	379	* a MOVE or GOTO command (but it can be used during a RUN command)
Davidroid	0:2887415a46cd	380	**********************************************************/
Davidroid	0:2887415a46cd	381	bool L6474::L6474_SetAcceleration(uint16_t newAcc)
Davidroid	0:2887415a46cd	382	{
Davidroid	0:2887415a46cd	383	bool cmdExecuted = FALSE;
Davidroid	0:2887415a46cd	384	if ((newAcc != 0)&&
Davidroid	18:2d6ab2b93685	385	((device_prm.motionState == INACTIVE)\|\|
Davidroid	18:2d6ab2b93685	386	(device_prm.commandExecuted == RUN_CMD)))
Davidroid	0:2887415a46cd	387	{
Davidroid	18:2d6ab2b93685	388	device_prm.acceleration = newAcc;
Davidroid	0:2887415a46cd	389	cmdExecuted = TRUE;
Davidroid	0:2887415a46cd	390	}
Davidroid	0:2887415a46cd	391	return cmdExecuted;
Davidroid	0:2887415a46cd	392	}
Davidroid	0:2887415a46cd	393
Davidroid	5:d3c78f12a78d	394	/**********************************************************
Davidroid	0:2887415a46cd	395	* @brief Changes the deceleration of the specified device
Davidroid	0:2887415a46cd	396	* @param[in] newDec New deceleration to apply in pps^2
Davidroid	0:2887415a46cd	397	* @retval true if the command is successfully executed, else false
Davidroid	0:2887415a46cd	398	* @note The command is not performed is the device is executing
Davidroid	0:2887415a46cd	399	* a MOVE or GOTO command (but it can be used during a RUN command)
Davidroid	0:2887415a46cd	400	**********************************************************/
Davidroid	0:2887415a46cd	401	bool L6474::L6474_SetDeceleration(uint16_t newDec)
Davidroid	0:2887415a46cd	402	{
Davidroid	0:2887415a46cd	403	bool cmdExecuted = FALSE;
Davidroid	0:2887415a46cd	404	if ((newDec != 0)&&
Davidroid	18:2d6ab2b93685	405	((device_prm.motionState == INACTIVE)\|\|
Davidroid	18:2d6ab2b93685	406	(device_prm.commandExecuted == RUN_CMD)))
Davidroid	0:2887415a46cd	407	{
Davidroid	18:2d6ab2b93685	408	device_prm.deceleration = newDec;
Davidroid	0:2887415a46cd	409	cmdExecuted = TRUE;
Davidroid	0:2887415a46cd	410	}
Davidroid	0:2887415a46cd	411	return cmdExecuted;
Davidroid	0:2887415a46cd	412	}
Davidroid	0:2887415a46cd	413
Davidroid	5:d3c78f12a78d	414	/**********************************************************
Davidroid	0:2887415a46cd	415	* @brief Set current position to be the Home position (ABS pos set to 0)
Davidroid	0:2887415a46cd	416	* @retval None
Davidroid	0:2887415a46cd	417	**********************************************************/
Davidroid	0:2887415a46cd	418	void L6474::L6474_SetHome(void)
Davidroid	0:2887415a46cd	419	{
Davidroid	0:2887415a46cd	420	L6474_CmdSetParam(L6474_ABS_POS, 0);
Davidroid	0:2887415a46cd	421	}
Davidroid	0:2887415a46cd	422
Davidroid	5:d3c78f12a78d	423	/**********************************************************
Davidroid	0:2887415a46cd	424	* @brief Sets current position to be the Mark position
Davidroid	0:2887415a46cd	425	* @retval None
Davidroid	0:2887415a46cd	426	**********************************************************/
Davidroid	0:2887415a46cd	427	void L6474::L6474_SetMark(void)
Davidroid	0:2887415a46cd	428	{
Davidroid	0:2887415a46cd	429	uint32_t mark = L6474_CmdGetParam(L6474_ABS_POS);
Davidroid	0:2887415a46cd	430	L6474_CmdSetParam(L6474_MARK, mark);
Davidroid	0:2887415a46cd	431	}
Davidroid	0:2887415a46cd	432
Davidroid	5:d3c78f12a78d	433	/**********************************************************
Davidroid	0:2887415a46cd	434	* @brief Changes the max speed of the specified device
Davidroid	0:2887415a46cd	435	* @param[in] newMaxSpeed New max speed to apply in pps
Davidroid	0:2887415a46cd	436	* @retval true if the command is successfully executed, else false
Davidroid	0:2887415a46cd	437	* @note The command is not performed is the device is executing
Davidroid	0:2887415a46cd	438	* a MOVE or GOTO command (but it can be used during a RUN command).
Davidroid	0:2887415a46cd	439	**********************************************************/
Davidroid	0:2887415a46cd	440	bool L6474::L6474_SetMaxSpeed(uint16_t newMaxSpeed)
Davidroid	0:2887415a46cd	441	{
Davidroid	0:2887415a46cd	442	bool cmdExecuted = FALSE;
Davidroid	0:2887415a46cd	443	if ((newMaxSpeed >= L6474_MIN_PWM_FREQ)&&
Davidroid	0:2887415a46cd	444	(newMaxSpeed <= L6474_MAX_PWM_FREQ) &&
Davidroid	18:2d6ab2b93685	445	(device_prm.minSpeed <= newMaxSpeed) &&
Davidroid	18:2d6ab2b93685	446	((device_prm.motionState == INACTIVE)\|\|
Davidroid	18:2d6ab2b93685	447	(device_prm.commandExecuted == RUN_CMD)))
Davidroid	0:2887415a46cd	448	{
Davidroid	18:2d6ab2b93685	449	device_prm.maxSpeed = newMaxSpeed;
Davidroid	0:2887415a46cd	450	cmdExecuted = TRUE;
Davidroid	0:2887415a46cd	451	}
Davidroid	0:2887415a46cd	452	return cmdExecuted;
Davidroid	0:2887415a46cd	453	}
Davidroid	0:2887415a46cd	454
Davidroid	5:d3c78f12a78d	455	/**********************************************************
Davidroid	0:2887415a46cd	456	* @brief Changes the min speed of the specified device
Davidroid	0:2887415a46cd	457	* @param[in] newMinSpeed New min speed to apply in pps
Davidroid	0:2887415a46cd	458	* @retval true if the command is successfully executed, else false
Davidroid	0:2887415a46cd	459	* @note The command is not performed is the device is executing
Davidroid	0:2887415a46cd	460	* a MOVE or GOTO command (but it can be used during a RUN command).
Davidroid	0:2887415a46cd	461	**********************************************************/
Davidroid	0:2887415a46cd	462	bool L6474::L6474_SetMinSpeed(uint16_t newMinSpeed)
Davidroid	0:2887415a46cd	463	{
Davidroid	0:2887415a46cd	464	bool cmdExecuted = FALSE;
Davidroid	0:2887415a46cd	465	if ((newMinSpeed >= L6474_MIN_PWM_FREQ)&&
Davidroid	0:2887415a46cd	466	(newMinSpeed <= L6474_MAX_PWM_FREQ) &&
Davidroid	18:2d6ab2b93685	467	(newMinSpeed <= device_prm.maxSpeed) &&
Davidroid	18:2d6ab2b93685	468	((device_prm.motionState == INACTIVE)\|\|
Davidroid	18:2d6ab2b93685	469	(device_prm.commandExecuted == RUN_CMD)))
Davidroid	0:2887415a46cd	470	{
Davidroid	18:2d6ab2b93685	471	device_prm.minSpeed = newMinSpeed;
Davidroid	0:2887415a46cd	472	cmdExecuted = TRUE;
Davidroid	0:2887415a46cd	473	}
Davidroid	0:2887415a46cd	474	return cmdExecuted;
Davidroid	0:2887415a46cd	475	}
Davidroid	0:2887415a46cd	476
Davidroid	5:d3c78f12a78d	477	/**********************************************************
Davidroid	0:2887415a46cd	478	* @brief Stops the motor by using the device deceleration
Davidroid	0:2887415a46cd	479	* @retval true if the command is successfully executed, else false
Davidroid	0:2887415a46cd	480	* @note The command is not performed is the device is in INACTIVE state.
Davidroid	0:2887415a46cd	481	**********************************************************/
Davidroid	0:2887415a46cd	482	bool L6474::L6474_SoftStop(void)
Davidroid	0:2887415a46cd	483	{
Davidroid	0:2887415a46cd	484	bool cmdExecuted = FALSE;
Davidroid	18:2d6ab2b93685	485	if (device_prm.motionState != INACTIVE)
Davidroid	0:2887415a46cd	486	{
Davidroid	18:2d6ab2b93685	487	device_prm.commandExecuted = SOFT_STOP_CMD;
Davidroid	0:2887415a46cd	488	cmdExecuted = TRUE;
Davidroid	0:2887415a46cd	489	}
Davidroid	0:2887415a46cd	490	return (cmdExecuted);
Davidroid	0:2887415a46cd	491	}
Davidroid	0:2887415a46cd	492
Davidroid	5:d3c78f12a78d	493	/**********************************************************
Davidroid	1:b38ebb8ea286	494	* @brief Locks until the device state becomes Inactive
Davidroid	1:b38ebb8ea286	495	* @retval None
Davidroid	1:b38ebb8ea286	496	**********************************************************/
Davidroid	1:b38ebb8ea286	497	void L6474::L6474_WaitWhileActive(void)
Davidroid	1:b38ebb8ea286	498	{
Davidroid	1:b38ebb8ea286	499	/* Wait while motor is running */
Davidroid	1:b38ebb8ea286	500	while (L6474_GetDeviceState() != INACTIVE);
Davidroid	1:b38ebb8ea286	501	}
Davidroid	1:b38ebb8ea286	502
Davidroid	5:d3c78f12a78d	503	/**********************************************************
Davidroid	0:2887415a46cd	504	* @brief Issue the Disable command to the L6474 of the specified device
Davidroid	0:2887415a46cd	505	* @retval None
Davidroid	0:2887415a46cd	506	**********************************************************/
Davidroid	0:2887415a46cd	507	void L6474::L6474_CmdDisable(void)
Davidroid	0:2887415a46cd	508	{
Davidroid	0:2887415a46cd	509	L6474_SendCommand(L6474_DISABLE);
Davidroid	0:2887415a46cd	510	}
Davidroid	0:2887415a46cd	511
Davidroid	5:d3c78f12a78d	512	/**********************************************************
Davidroid	0:2887415a46cd	513	* @brief Issues the Enable command to the L6474 of the specified device
Davidroid	0:2887415a46cd	514	* @retval None
Davidroid	0:2887415a46cd	515	**********************************************************/
Davidroid	0:2887415a46cd	516	void L6474::L6474_CmdEnable(void)
Davidroid	0:2887415a46cd	517	{
Davidroid	0:2887415a46cd	518	L6474_SendCommand(L6474_ENABLE);
Davidroid	0:2887415a46cd	519	}
Davidroid	0:2887415a46cd	520
Davidroid	5:d3c78f12a78d	521	/**********************************************************
Davidroid	0:2887415a46cd	522	* @brief Issues the GetParam command to the L6474 of the specified device
Davidroid	7:f7e0c3621f77	523	* @param[in] parameter Register adress (L6474_ABS_POS, L6474_MARK,...)
Davidroid	0:2887415a46cd	524	* @retval Register value
Davidroid	0:2887415a46cd	525	**********************************************************/
Davidroid	7:f7e0c3621f77	526	uint32_t L6474::L6474_CmdGetParam(L6474_Registers_t parameter)
Davidroid	0:2887415a46cd	527	{
Davidroid	0:2887415a46cd	528	uint32_t i;
Davidroid	0:2887415a46cd	529	uint32_t spiRxData;
Davidroid	0:2887415a46cd	530	uint8_t maxArgumentNbBytes = 0;
Davidroid	18:2d6ab2b93685	531	uint8_t spiIndex = number_of_devices - device_instance - 1;
Davidroid	0:2887415a46cd	532	bool itDisable = FALSE;
Davidroid	0:2887415a46cd	533
Davidroid	0:2887415a46cd	534	do
Davidroid	0:2887415a46cd	535	{
Davidroid	18:2d6ab2b93685	536	spi_preemtion_by_isr = FALSE;
Davidroid	0:2887415a46cd	537	if (itDisable)
Davidroid	0:2887415a46cd	538	{
Davidroid	0:2887415a46cd	539	/* re-enable L6474_EnableIrq if disable in previous iteration */
Davidroid	0:2887415a46cd	540	L6474_EnableIrq();
Davidroid	0:2887415a46cd	541	itDisable = FALSE;
Davidroid	0:2887415a46cd	542	}
Davidroid	0:2887415a46cd	543
Davidroid	18:2d6ab2b93685	544	for (i = 0; i < number_of_devices; i++)
Davidroid	5:d3c78f12a78d	545	{
Davidroid	18:2d6ab2b93685	546	spi_tx_bursts[0][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	547	spi_tx_bursts[1][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	548	spi_tx_bursts[2][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	549	spi_tx_bursts[3][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	550	spi_rx_bursts[1][i] = 0;
Davidroid	18:2d6ab2b93685	551	spi_rx_bursts[2][i] = 0;
Davidroid	18:2d6ab2b93685	552	spi_rx_bursts[3][i] = 0;
Davidroid	5:d3c78f12a78d	553	}
Davidroid	0:2887415a46cd	554
Davidroid	7:f7e0c3621f77	555	switch (parameter)
Davidroid	0:2887415a46cd	556	{
Davidroid	0:2887415a46cd	557	case L6474_ABS_POS: ;
Davidroid	0:2887415a46cd	558	case L6474_MARK:
Davidroid	18:2d6ab2b93685	559	spi_tx_bursts[0][spiIndex] = ((uint8_t)L6474_GET_PARAM )\| (parameter);
Davidroid	0:2887415a46cd	560	maxArgumentNbBytes = 3;
Davidroid	0:2887415a46cd	561	break;
Davidroid	0:2887415a46cd	562	case L6474_EL_POS: ;
Davidroid	0:2887415a46cd	563	case L6474_CONFIG: ;
Davidroid	0:2887415a46cd	564	case L6474_STATUS:
Davidroid	18:2d6ab2b93685	565	spi_tx_bursts[1][spiIndex] = ((uint8_t)L6474_GET_PARAM )\| (parameter);
Davidroid	0:2887415a46cd	566	maxArgumentNbBytes = 2;
Davidroid	0:2887415a46cd	567	break;
Davidroid	0:2887415a46cd	568	default:
Davidroid	18:2d6ab2b93685	569	spi_tx_bursts[2][spiIndex] = ((uint8_t)L6474_GET_PARAM )\| (parameter);
Davidroid	0:2887415a46cd	570	maxArgumentNbBytes = 1;
Davidroid	0:2887415a46cd	571	}
Davidroid	0:2887415a46cd	572
Davidroid	0:2887415a46cd	573	/* Disable interruption before checking */
Davidroid	0:2887415a46cd	574	/* pre-emption by ISR and SPI transfers*/
Davidroid	0:2887415a46cd	575	L6474_DisableIrq();
Davidroid	0:2887415a46cd	576	itDisable = TRUE;
Davidroid	18:2d6ab2b93685	577	} while (spi_preemtion_by_isr); // check pre-emption by ISR
Davidroid	0:2887415a46cd	578
Davidroid	0:2887415a46cd	579	for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes;
Davidroid	0:2887415a46cd	580	i < L6474_CMD_ARG_MAX_NB_BYTES;
Davidroid	0:2887415a46cd	581	i++)
Davidroid	0:2887415a46cd	582	{
Davidroid	18:2d6ab2b93685	583	L6474_WriteBytes(&spi_tx_bursts[i][0], &spi_rx_bursts[i][0]);
Davidroid	0:2887415a46cd	584	}
Davidroid	0:2887415a46cd	585
Davidroid	18:2d6ab2b93685	586	spiRxData = ((uint32_t)spi_rx_bursts[1][spiIndex] << 16) \|
Davidroid	18:2d6ab2b93685	587	(spi_rx_bursts[2][spiIndex] << 8) \|
Davidroid	18:2d6ab2b93685	588	(spi_rx_bursts[3][spiIndex]);
Davidroid	0:2887415a46cd	589
Davidroid	0:2887415a46cd	590	/* re-enable L6474_EnableIrq after SPI transfers*/
Davidroid	0:2887415a46cd	591	L6474_EnableIrq();
Davidroid	0:2887415a46cd	592
Davidroid	0:2887415a46cd	593	return (spiRxData);
Davidroid	0:2887415a46cd	594	}
Davidroid	0:2887415a46cd	595
Davidroid	5:d3c78f12a78d	596	/**********************************************************
Davidroid	0:2887415a46cd	597	* @brief Issues the GetStatus command to the L6474 of the specified device
Davidroid	0:2887415a46cd	598	* @retval Status Register value
Davidroid	0:2887415a46cd	599	* @note Once the GetStatus command is performed, the flags of the status register
Davidroid	0:2887415a46cd	600	* are reset. This is not the case when the status register is read with the
Davidroid	7:f7e0c3621f77	601	* GetParam command (via the functions L6474ReadStatusRegister or L6474_CmdGetParam).
Davidroid	0:2887415a46cd	602	**********************************************************/
Davidroid	0:2887415a46cd	603	uint16_t L6474::L6474_CmdGetStatus(void)
Davidroid	0:2887415a46cd	604	{
Davidroid	0:2887415a46cd	605	uint32_t i;
Davidroid	0:2887415a46cd	606	uint16_t status;
Davidroid	18:2d6ab2b93685	607	uint8_t spiIndex = number_of_devices - device_instance - 1;
Davidroid	0:2887415a46cd	608	bool itDisable = FALSE;
Davidroid	0:2887415a46cd	609
Davidroid	0:2887415a46cd	610	do
Davidroid	0:2887415a46cd	611	{
Davidroid	18:2d6ab2b93685	612	spi_preemtion_by_isr = FALSE;
Davidroid	0:2887415a46cd	613	if (itDisable)
Davidroid	0:2887415a46cd	614	{
Davidroid	0:2887415a46cd	615	/* re-enable L6474_EnableIrq if disable in previous iteration */
Davidroid	0:2887415a46cd	616	L6474_EnableIrq();
Davidroid	0:2887415a46cd	617	itDisable = FALSE;
Davidroid	0:2887415a46cd	618	}
Davidroid	0:2887415a46cd	619
Davidroid	18:2d6ab2b93685	620	for (i = 0; i < number_of_devices; i++)
Davidroid	5:d3c78f12a78d	621	{
Davidroid	18:2d6ab2b93685	622	spi_tx_bursts[0][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	623	spi_tx_bursts[1][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	624	spi_tx_bursts[2][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	625	spi_rx_bursts[1][i] = 0;
Davidroid	18:2d6ab2b93685	626	spi_rx_bursts[2][i] = 0;
Davidroid	5:d3c78f12a78d	627	}
Davidroid	18:2d6ab2b93685	628	spi_tx_bursts[0][spiIndex] = L6474_GET_STATUS;
Davidroid	0:2887415a46cd	629
Davidroid	0:2887415a46cd	630	/* Disable interruption before checking */
Davidroid	0:2887415a46cd	631	/* pre-emption by ISR and SPI transfers*/
Davidroid	0:2887415a46cd	632	L6474_DisableIrq();
Davidroid	0:2887415a46cd	633	itDisable = TRUE;
Davidroid	18:2d6ab2b93685	634	} while (spi_preemtion_by_isr); // check pre-emption by ISR
Davidroid	0:2887415a46cd	635
Davidroid	0:2887415a46cd	636	for (i = 0; i < L6474_CMD_ARG_NB_BYTES_GET_STATUS + L6474_RSP_NB_BYTES_GET_STATUS; i++)
Davidroid	0:2887415a46cd	637	{
Davidroid	18:2d6ab2b93685	638	L6474_WriteBytes(&spi_tx_bursts[i][0], &spi_rx_bursts[i][0]);
Davidroid	0:2887415a46cd	639	}
Davidroid	18:2d6ab2b93685	640	status = (spi_rx_bursts[1][spiIndex] << 8) \| (spi_rx_bursts[2][spiIndex]);
Davidroid	0:2887415a46cd	641
Davidroid	0:2887415a46cd	642	/* re-enable L6474_EnableIrq after SPI transfers*/
Davidroid	0:2887415a46cd	643	L6474_EnableIrq();
Davidroid	0:2887415a46cd	644
Davidroid	0:2887415a46cd	645	return (status);
Davidroid	0:2887415a46cd	646	}
Davidroid	0:2887415a46cd	647
Davidroid	5:d3c78f12a78d	648	/**********************************************************
Davidroid	0:2887415a46cd	649	* @brief Issues the Nop command to the L6474 of the specified device
Davidroid	0:2887415a46cd	650	* @retval None
Davidroid	0:2887415a46cd	651	**********************************************************/
Davidroid	0:2887415a46cd	652	void L6474::L6474_CmdNop(void)
Davidroid	0:2887415a46cd	653	{
Davidroid	0:2887415a46cd	654	L6474_SendCommand(L6474_NOP);
Davidroid	0:2887415a46cd	655	}
Davidroid	0:2887415a46cd	656
Davidroid	5:d3c78f12a78d	657	/**********************************************************
Davidroid	0:2887415a46cd	658	* @brief Issues the SetParam command to the L6474 of the specified device
Davidroid	7:f7e0c3621f77	659	* @param[in] parameter Register adress (L6474_ABS_POS, L6474_MARK,...)
Davidroid	0:2887415a46cd	660	* @param[in] value Value to set in the register
Davidroid	0:2887415a46cd	661	* @retval None
Davidroid	0:2887415a46cd	662	**********************************************************/
Davidroid	7:f7e0c3621f77	663	void L6474::L6474_CmdSetParam(L6474_Registers_t parameter, uint32_t value)
Davidroid	0:2887415a46cd	664	{
Davidroid	0:2887415a46cd	665	uint32_t i;
Davidroid	0:2887415a46cd	666	uint8_t maxArgumentNbBytes = 0;
Davidroid	18:2d6ab2b93685	667	uint8_t spiIndex = number_of_devices - device_instance - 1;
Davidroid	0:2887415a46cd	668	bool itDisable = FALSE;
Davidroid	0:2887415a46cd	669	do
Davidroid	0:2887415a46cd	670	{
Davidroid	18:2d6ab2b93685	671	spi_preemtion_by_isr = FALSE;
Davidroid	0:2887415a46cd	672	if (itDisable)
Davidroid	0:2887415a46cd	673	{
Davidroid	0:2887415a46cd	674	/* re-enable L6474_EnableIrq if disable in previous iteration */
Davidroid	0:2887415a46cd	675	L6474_EnableIrq();
Davidroid	0:2887415a46cd	676	itDisable = FALSE;
Davidroid	0:2887415a46cd	677	}
Davidroid	0:2887415a46cd	678
Davidroid	18:2d6ab2b93685	679	for (i = 0; i < number_of_devices; i++)
Davidroid	5:d3c78f12a78d	680	{
Davidroid	18:2d6ab2b93685	681	spi_tx_bursts[0][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	682	spi_tx_bursts[1][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	683	spi_tx_bursts[2][i] = L6474_NOP;
Davidroid	18:2d6ab2b93685	684	spi_tx_bursts[3][i] = L6474_NOP;
Davidroid	5:d3c78f12a78d	685	}
Davidroid	0:2887415a46cd	686
Davidroid	7:f7e0c3621f77	687	switch (parameter)
Davidroid	0:2887415a46cd	688	{
Davidroid	0:2887415a46cd	689	case L6474_ABS_POS: ;
Davidroid	0:2887415a46cd	690	case L6474_MARK:
Davidroid	18:2d6ab2b93685	691	spi_tx_bursts[0][spiIndex] = parameter;
Davidroid	18:2d6ab2b93685	692	spi_tx_bursts[1][spiIndex] = (uint8_t)(value >> 16);
Davidroid	18:2d6ab2b93685	693	spi_tx_bursts[2][spiIndex] = (uint8_t)(value >> 8);
Davidroid	0:2887415a46cd	694	maxArgumentNbBytes = 3;
Davidroid	0:2887415a46cd	695	break;
Davidroid	0:2887415a46cd	696	case L6474_EL_POS: ;
Davidroid	0:2887415a46cd	697	case L6474_CONFIG:
Davidroid	18:2d6ab2b93685	698	spi_tx_bursts[1][spiIndex] = parameter;
Davidroid	18:2d6ab2b93685	699	spi_tx_bursts[2][spiIndex] = (uint8_t)(value >> 8);
Davidroid	0:2887415a46cd	700	maxArgumentNbBytes = 2;
Davidroid	0:2887415a46cd	701	break;
Davidroid	0:2887415a46cd	702	default:
Davidroid	18:2d6ab2b93685	703	spi_tx_bursts[2][spiIndex] = parameter;
Davidroid	0:2887415a46cd	704	maxArgumentNbBytes = 1;
Davidroid	0:2887415a46cd	705	break;
Davidroid	0:2887415a46cd	706	}
Davidroid	18:2d6ab2b93685	707	spi_tx_bursts[3][spiIndex] = (uint8_t)(value);
Davidroid	0:2887415a46cd	708
Davidroid	0:2887415a46cd	709	/* Disable interruption before checking */
Davidroid	0:2887415a46cd	710	/* pre-emption by ISR and SPI transfers*/
Davidroid	0:2887415a46cd	711	L6474_DisableIrq();
Davidroid	0:2887415a46cd	712	itDisable = TRUE;
Davidroid	18:2d6ab2b93685	713	} while (spi_preemtion_by_isr); // check pre-emption by ISR
Davidroid	0:2887415a46cd	714
Davidroid	0:2887415a46cd	715	/* SPI transfer */
Davidroid	0:2887415a46cd	716	for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes;
Davidroid	0:2887415a46cd	717	i < L6474_CMD_ARG_MAX_NB_BYTES;
Davidroid	0:2887415a46cd	718	i++)
Davidroid	0:2887415a46cd	719	{
Davidroid	18:2d6ab2b93685	720	L6474_WriteBytes(&spi_tx_bursts[i][0],&spi_rx_bursts[i][0]);
Davidroid	0:2887415a46cd	721	}
Davidroid	0:2887415a46cd	722	/* re-enable L6474_EnableIrq after SPI transfers*/
Davidroid	0:2887415a46cd	723	L6474_EnableIrq();
Davidroid	0:2887415a46cd	724	}
Davidroid	0:2887415a46cd	725
Davidroid	5:d3c78f12a78d	726	/**********************************************************
Davidroid	0:2887415a46cd	727	* @brief Reads the Status Register value
Davidroid	0:2887415a46cd	728	* @retval Status register valued
Davidroid	0:2887415a46cd	729	* @note The status register flags are not cleared
Davidroid	0:2887415a46cd	730	* at the difference with L6474CmdGetStatus()
Davidroid	0:2887415a46cd	731	**********************************************************/
Davidroid	0:2887415a46cd	732	uint16_t L6474::L6474_ReadStatusRegister(void)
Davidroid	0:2887415a46cd	733	{
Davidroid	0:2887415a46cd	734	return (L6474_CmdGetParam(L6474_STATUS));
Davidroid	0:2887415a46cd	735	}
Davidroid	0:2887415a46cd	736
Davidroid	5:d3c78f12a78d	737	/**********************************************************
Davidroid	0:2887415a46cd	738	* @brief Set the stepping mode
Davidroid	0:2887415a46cd	739	* @param[in] stepMod from full step to 1/16 microstep as specified in enum motorStepMode_t
Davidroid	0:2887415a46cd	740	* @retval None
Davidroid	0:2887415a46cd	741	**********************************************************/
Davidroid	0:2887415a46cd	742	void L6474::L6474_SelectStepMode(motorStepMode_t stepMod)
Davidroid	0:2887415a46cd	743	{
Davidroid	0:2887415a46cd	744	uint8_t stepModeRegister;
Davidroid	0:2887415a46cd	745	L6474_STEP_SEL_t l6474StepMod;
Davidroid	0:2887415a46cd	746
Davidroid	0:2887415a46cd	747	switch (stepMod)
Davidroid	0:2887415a46cd	748	{
Davidroid	0:2887415a46cd	749	case STEP_MODE_FULL:
Davidroid	0:2887415a46cd	750	l6474StepMod = L6474_STEP_SEL_1;
Davidroid	0:2887415a46cd	751	break;
Davidroid	0:2887415a46cd	752	case STEP_MODE_HALF:
Davidroid	0:2887415a46cd	753	l6474StepMod = L6474_STEP_SEL_1_2;
Davidroid	0:2887415a46cd	754	break;
Davidroid	0:2887415a46cd	755	case STEP_MODE_1_4:
Davidroid	0:2887415a46cd	756	l6474StepMod = L6474_STEP_SEL_1_4;
Davidroid	0:2887415a46cd	757	break;
Davidroid	0:2887415a46cd	758	case STEP_MODE_1_8:
Davidroid	0:2887415a46cd	759	l6474StepMod = L6474_STEP_SEL_1_8;
Davidroid	0:2887415a46cd	760	break;
Davidroid	0:2887415a46cd	761	case STEP_MODE_1_16:
Davidroid	0:2887415a46cd	762	default:
Davidroid	0:2887415a46cd	763	l6474StepMod = L6474_STEP_SEL_1_16;
Davidroid	0:2887415a46cd	764	break;
Davidroid	0:2887415a46cd	765	}
Davidroid	0:2887415a46cd	766
Davidroid	0:2887415a46cd	767	/* Eventually deactivate motor */
Davidroid	18:2d6ab2b93685	768	if (device_prm.motionState != INACTIVE)
Davidroid	0:2887415a46cd	769	{
Davidroid	0:2887415a46cd	770	L6474_HardStop();
Davidroid	0:2887415a46cd	771	}
Davidroid	0:2887415a46cd	772
Davidroid	0:2887415a46cd	773	/* Read Step mode register and clear STEP_SEL field */
Davidroid	0:2887415a46cd	774	stepModeRegister = (uint8_t)(0xF8 & L6474_CmdGetParam(L6474_STEP_MODE)) ;
Davidroid	0:2887415a46cd	775
Davidroid	0:2887415a46cd	776	/* Apply new step mode */
Davidroid	0:2887415a46cd	777	L6474_CmdSetParam(L6474_STEP_MODE, stepModeRegister \| (uint8_t)l6474StepMod);
Davidroid	0:2887415a46cd	778
Davidroid	0:2887415a46cd	779	/* Reset abs pos register */
Davidroid	0:2887415a46cd	780	L6474_SetHome();
Davidroid	0:2887415a46cd	781	}
Davidroid	0:2887415a46cd	782
Davidroid	5:d3c78f12a78d	783	/**********************************************************
Davidroid	5:d3c78f12a78d	784	* @brief Get the direction
Davidroid	5:d3c78f12a78d	785	* @param None
Davidroid	5:d3c78f12a78d	786	* @retval direction FORWARD or BACKWARD
Davidroid	4:83a1eb397a65	787	**********************************************************/
Davidroid	5:d3c78f12a78d	788	motorDir_t L6474::L6474_GetDirection(void)
Davidroid	4:83a1eb397a65	789	{
Davidroid	18:2d6ab2b93685	790	return device_prm.direction;
Davidroid	4:83a1eb397a65	791	}
Davidroid	4:83a1eb397a65	792
Davidroid	5:d3c78f12a78d	793	/**********************************************************
Davidroid	0:2887415a46cd	794	* @brief Specifies the direction
Davidroid	0:2887415a46cd	795	* @param[in] dir FORWARD or BACKWARD
Davidroid	0:2887415a46cd	796	* @note The direction change is only applied if the device
Davidroid	0:2887415a46cd	797	* is in INACTIVE state
Davidroid	0:2887415a46cd	798	* @retval None
Davidroid	0:2887415a46cd	799	**********************************************************/
Davidroid	5:d3c78f12a78d	800	void L6474::L6474_SetDirection(motorDir_t direction)
Davidroid	0:2887415a46cd	801	{
Davidroid	18:2d6ab2b93685	802	if (device_prm.motionState == INACTIVE)
Davidroid	0:2887415a46cd	803	{
Davidroid	18:2d6ab2b93685	804	device_prm.direction = direction;
Davidroid	5:d3c78f12a78d	805	L6474_SetDirectionGpio(direction);
Davidroid	0:2887415a46cd	806	}
Davidroid	0:2887415a46cd	807	}
Davidroid	0:2887415a46cd	808
Davidroid	5:d3c78f12a78d	809	/**********************************************************
Davidroid	0:2887415a46cd	810	* @brief Updates the current speed of the device
Davidroid	0:2887415a46cd	811	* @param[in] newSpeed in pps
Davidroid	0:2887415a46cd	812	* @retval None
Davidroid	0:2887415a46cd	813	**********************************************************/
Davidroid	0:2887415a46cd	814	void L6474::L6474_ApplySpeed(uint16_t newSpeed)
Davidroid	0:2887415a46cd	815	{
Davidroid	0:2887415a46cd	816	if (newSpeed < L6474_MIN_PWM_FREQ)
Davidroid	0:2887415a46cd	817	{
Davidroid	0:2887415a46cd	818	newSpeed = L6474_MIN_PWM_FREQ;
Davidroid	0:2887415a46cd	819	}
Davidroid	0:2887415a46cd	820	if (newSpeed > L6474_MAX_PWM_FREQ)
Davidroid	0:2887415a46cd	821	{
Davidroid	0:2887415a46cd	822	newSpeed = L6474_MAX_PWM_FREQ;
Davidroid	0:2887415a46cd	823	}
Davidroid	0:2887415a46cd	824
Davidroid	18:2d6ab2b93685	825	device_prm.speed = newSpeed;
Davidroid	0:2887415a46cd	826
Davidroid	0:2887415a46cd	827	L6474_PwmSetFreq(newSpeed);
Davidroid	0:2887415a46cd	828	}
Davidroid	0:2887415a46cd	829
Davidroid	5:d3c78f12a78d	830	/**********************************************************
Davidroid	0:2887415a46cd	831	* @brief Computes the speed profile according to the number of steps to move
Davidroid	0:2887415a46cd	832	* @param[in] nbSteps number of steps to perform
Davidroid	0:2887415a46cd	833	* @retval None
Davidroid	0:2887415a46cd	834	* @note Using the acceleration and deceleration of the device,
Davidroid	0:2887415a46cd	835	* this function determines the duration in steps of the acceleration,
Davidroid	0:2887415a46cd	836	* steady and deceleration phases.
Davidroid	0:2887415a46cd	837	* If the total number of steps to perform is big enough, a trapezoidal move
Davidroid	0:2887415a46cd	838	* is performed (i.e. there is a steady phase where the motor runs at the maximum
Davidroid	0:2887415a46cd	839	* speed.
Davidroid	0:2887415a46cd	840	* Else, a triangular move is performed (no steady phase: the maximum speed is never
Davidroid	0:2887415a46cd	841	* reached.
Davidroid	0:2887415a46cd	842	**********************************************************/
Davidroid	0:2887415a46cd	843	void L6474::L6474_ComputeSpeedProfile(uint32_t nbSteps)
Davidroid	0:2887415a46cd	844	{
Davidroid	0:2887415a46cd	845	uint32_t reqAccSteps;
Davidroid	0:2887415a46cd	846	uint32_t reqDecSteps;
Davidroid	0:2887415a46cd	847
Davidroid	0:2887415a46cd	848	/* compute the number of steps to get the targeted speed */
Davidroid	18:2d6ab2b93685	849	uint16_t minSpeed = device_prm.minSpeed;
Davidroid	18:2d6ab2b93685	850	reqAccSteps = (device_prm.maxSpeed - minSpeed);
Davidroid	18:2d6ab2b93685	851	reqAccSteps *= (device_prm.maxSpeed + minSpeed);
Davidroid	0:2887415a46cd	852	reqDecSteps = reqAccSteps;
Davidroid	18:2d6ab2b93685	853	reqAccSteps /= (uint32_t)device_prm.acceleration;
Davidroid	0:2887415a46cd	854	reqAccSteps /= 2;
Davidroid	0:2887415a46cd	855
Davidroid	0:2887415a46cd	856	/* compute the number of steps to stop */
Davidroid	18:2d6ab2b93685	857	reqDecSteps /= (uint32_t)device_prm.deceleration;
Davidroid	0:2887415a46cd	858	reqDecSteps /= 2;
Davidroid	0:2887415a46cd	859
Davidroid	0:2887415a46cd	860	if(( reqAccSteps + reqDecSteps ) > nbSteps)
Davidroid	0:2887415a46cd	861	{
Davidroid	0:2887415a46cd	862	/* Triangular move */
Davidroid	0:2887415a46cd	863	/* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
Davidroid	18:2d6ab2b93685	864	uint32_t dec = device_prm.deceleration;
Davidroid	18:2d6ab2b93685	865	uint32_t acc = device_prm.acceleration;
Davidroid	0:2887415a46cd	866
Davidroid	0:2887415a46cd	867	reqDecSteps = ((uint32_t) dec * nbSteps) / (acc + dec);
Davidroid	0:2887415a46cd	868	if (reqDecSteps > 1)
Davidroid	0:2887415a46cd	869	{
Davidroid	0:2887415a46cd	870	reqAccSteps = reqDecSteps - 1;
Davidroid	0:2887415a46cd	871	if(reqAccSteps == 0)
Davidroid	0:2887415a46cd	872	{
Davidroid	0:2887415a46cd	873	reqAccSteps = 1;
Davidroid	0:2887415a46cd	874	}
Davidroid	0:2887415a46cd	875	}
Davidroid	0:2887415a46cd	876	else
Davidroid	0:2887415a46cd	877	{
Davidroid	0:2887415a46cd	878	reqAccSteps = 0;
Davidroid	0:2887415a46cd	879	}
Davidroid	18:2d6ab2b93685	880	device_prm.endAccPos = reqAccSteps;
Davidroid	18:2d6ab2b93685	881	device_prm.startDecPos = reqDecSteps;
Davidroid	0:2887415a46cd	882	}
Davidroid	0:2887415a46cd	883	else
Davidroid	0:2887415a46cd	884	{
Davidroid	0:2887415a46cd	885	/* Trapezoidal move */
Davidroid	0:2887415a46cd	886	/* accelerating phase to endAccPos */
Davidroid	0:2887415a46cd	887	/* steady phase from endAccPos to startDecPos */
Davidroid	0:2887415a46cd	888	/* decelerating from startDecPos to stepsToTake*/
Davidroid	18:2d6ab2b93685	889	device_prm.endAccPos = reqAccSteps;
Davidroid	18:2d6ab2b93685	890	device_prm.startDecPos = nbSteps - reqDecSteps - 1;
Davidroid	0:2887415a46cd	891	}
Davidroid	0:2887415a46cd	892	}
Davidroid	0:2887415a46cd	893
Davidroid	5:d3c78f12a78d	894	/**********************************************************
Davidroid	0:2887415a46cd	895	* @brief Converts the ABS_POSITION register value to a 32b signed integer
Davidroid	0:2887415a46cd	896	* @param[in] abs_position_reg value of the ABS_POSITION register
Davidroid	0:2887415a46cd	897	* @retval operation_result 32b signed integer corresponding to the absolute position
Davidroid	0:2887415a46cd	898	**********************************************************/
Davidroid	0:2887415a46cd	899	int32_t L6474::L6474_ConvertPosition(uint32_t abs_position_reg)
Davidroid	0:2887415a46cd	900	{
Davidroid	0:2887415a46cd	901	int32_t operation_result;
Davidroid	0:2887415a46cd	902
Davidroid	0:2887415a46cd	903	if (abs_position_reg & L6474_ABS_POS_SIGN_BIT_MASK)
Davidroid	0:2887415a46cd	904	{
Davidroid	0:2887415a46cd	905	/* Negative register value */
Davidroid	0:2887415a46cd	906	abs_position_reg = ~abs_position_reg;
Davidroid	0:2887415a46cd	907	abs_position_reg += 1;
Davidroid	0:2887415a46cd	908
Davidroid	0:2887415a46cd	909	operation_result = (int32_t) (abs_position_reg & L6474_ABS_POS_VALUE_MASK);
Davidroid	0:2887415a46cd	910	operation_result = -operation_result;
Davidroid	0:2887415a46cd	911	}
Davidroid	0:2887415a46cd	912	else
Davidroid	0:2887415a46cd	913	{
Davidroid	0:2887415a46cd	914	operation_result = (int32_t) abs_position_reg;
Davidroid	0:2887415a46cd	915	}
Davidroid	5:d3c78f12a78d	916
Davidroid	0:2887415a46cd	917	return operation_result;
Davidroid	0:2887415a46cd	918	}
Davidroid	0:2887415a46cd	919
Davidroid	5:d3c78f12a78d	920	/**********************************************************
Davidroid	0:2887415a46cd	921	* @brief Error handler which calls the user callback (if defined)
Davidroid	0:2887415a46cd	922	* @param[in] error Number of the error
Davidroid	0:2887415a46cd	923	* @retval None
Davidroid	0:2887415a46cd	924	**********************************************************/
Davidroid	0:2887415a46cd	925	void L6474::L6474_ErrorHandler(uint16_t error)
Davidroid	0:2887415a46cd	926	{
Davidroid	18:2d6ab2b93685	927	if (error_handler_callback != 0)
Davidroid	0:2887415a46cd	928	{
Davidroid	18:2d6ab2b93685	929	(void) error_handler_callback(error);
Davidroid	0:2887415a46cd	930	}
Davidroid	0:2887415a46cd	931	else
Davidroid	0:2887415a46cd	932	{
Davidroid	7:f7e0c3621f77	933	/* Aborting the program. */
Davidroid	7:f7e0c3621f77	934	exit(EXIT_FAILURE);
Davidroid	0:2887415a46cd	935	}
Davidroid	0:2887415a46cd	936	}
Davidroid	0:2887415a46cd	937
Davidroid	5:d3c78f12a78d	938	/**********************************************************
Davidroid	0:2887415a46cd	939	* @brief Sends a command without arguments to the L6474 via the SPI
Davidroid	0:2887415a46cd	940	* @param[in] param Command to send
Davidroid	0:2887415a46cd	941	* @retval None
Davidroid	0:2887415a46cd	942	**********************************************************/
Davidroid	0:2887415a46cd	943	void L6474::L6474_SendCommand(uint8_t param)
Davidroid	0:2887415a46cd	944	{
Davidroid	5:d3c78f12a78d	945	uint32_t i;
Davidroid	5:d3c78f12a78d	946	bool itDisable = FALSE;
Davidroid	18:2d6ab2b93685	947	uint8_t spiIndex = number_of_devices - device_instance - 1;
Davidroid	0:2887415a46cd	948
Davidroid	0:2887415a46cd	949	do
Davidroid	0:2887415a46cd	950	{
Davidroid	18:2d6ab2b93685	951	spi_preemtion_by_isr = FALSE;
Davidroid	0:2887415a46cd	952	if (itDisable)
Davidroid	0:2887415a46cd	953	{
Davidroid	0:2887415a46cd	954	/* re-enable L6474_EnableIrq if disable in previous iteration */
Davidroid	0:2887415a46cd	955	L6474_EnableIrq();
Davidroid	0:2887415a46cd	956	itDisable = FALSE;
Davidroid	0:2887415a46cd	957	}
Davidroid	0:2887415a46cd	958
Davidroid	18:2d6ab2b93685	959	for (i = 0; i < number_of_devices; i++)
Davidroid	5:d3c78f12a78d	960	{
Davidroid	18:2d6ab2b93685	961	spi_tx_bursts[3][i] = L6474_NOP;
Davidroid	5:d3c78f12a78d	962	}
Davidroid	18:2d6ab2b93685	963	spi_tx_bursts[3][spiIndex] = param;
Davidroid	0:2887415a46cd	964
Davidroid	0:2887415a46cd	965	/* Disable interruption before checking */
Davidroid	0:2887415a46cd	966	/* pre-emption by ISR and SPI transfers*/
Davidroid	0:2887415a46cd	967	L6474_DisableIrq();
Davidroid	0:2887415a46cd	968	itDisable = TRUE;
Davidroid	18:2d6ab2b93685	969	} while (spi_preemtion_by_isr); // check pre-emption by ISR
Davidroid	0:2887415a46cd	970
Davidroid	18:2d6ab2b93685	971	L6474_WriteBytes(&spi_tx_bursts[3][0], &spi_rx_bursts[3][0]);
Davidroid	0:2887415a46cd	972
Davidroid	0:2887415a46cd	973	/* re-enable L6474_EnableIrq after SPI transfers*/
Davidroid	0:2887415a46cd	974	L6474_EnableIrq();
Davidroid	0:2887415a46cd	975	}
Davidroid	0:2887415a46cd	976
Davidroid	5:d3c78f12a78d	977	/**********************************************************
Davidroid	0:2887415a46cd	978	* @brief Sets the registers of the L6474 to their predefined values
Davidroid	0:2887415a46cd	979	* from l6474_target_config.h
Davidroid	0:2887415a46cd	980	* @retval None
Davidroid	0:2887415a46cd	981	**********************************************************/
Davidroid	0:2887415a46cd	982	void L6474::L6474_SetRegisterToPredefinedValues(void)
Davidroid	0:2887415a46cd	983	{
Davidroid	0:2887415a46cd	984	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	985	L6474_ABS_POS,
Davidroid	0:2887415a46cd	986	0);
Davidroid	0:2887415a46cd	987	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	988	L6474_EL_POS,
Davidroid	0:2887415a46cd	989	0);
Davidroid	0:2887415a46cd	990	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	991	L6474_MARK,
Davidroid	0:2887415a46cd	992	0);
Davidroid	18:2d6ab2b93685	993	switch (device_instance)
Davidroid	0:2887415a46cd	994	{
Davidroid	0:2887415a46cd	995	case 0:
Davidroid	0:2887415a46cd	996	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	997	L6474_TVAL,
Davidroid	0:2887415a46cd	998	L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0));
Davidroid	0:2887415a46cd	999	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1000	L6474_T_FAST,
Davidroid	0:2887415a46cd	1001	(uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_0 \|
Davidroid	0:2887415a46cd	1002	(uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_0);
Davidroid	0:2887415a46cd	1003	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1004	L6474_TON_MIN,
Davidroid	0:2887415a46cd	1005	L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_0)
Davidroid	0:2887415a46cd	1006	);
Davidroid	0:2887415a46cd	1007	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1008	L6474_TOFF_MIN,
Davidroid	0:2887415a46cd	1009	L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_0));
Davidroid	0:2887415a46cd	1010	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1011	L6474_OCD_TH,
Davidroid	0:2887415a46cd	1012	L6474_CONF_PARAM_OCD_TH_DEVICE_0);
Davidroid	0:2887415a46cd	1013	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1014	L6474_STEP_MODE,
Davidroid	0:2887415a46cd	1015	(uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_0 \|
Davidroid	0:2887415a46cd	1016	(uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_0);
Davidroid	0:2887415a46cd	1017	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1018	L6474_ALARM_EN,
Davidroid	0:2887415a46cd	1019	L6474_CONF_PARAM_ALARM_EN_DEVICE_0);
Davidroid	0:2887415a46cd	1020	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1021	L6474_CONFIG,
Davidroid	0:2887415a46cd	1022	(uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_0 \|
Davidroid	0:2887415a46cd	1023	(uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_0 \|
Davidroid	0:2887415a46cd	1024	(uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_0 \|
Davidroid	0:2887415a46cd	1025	(uint16_t)L6474_CONF_PARAM_SR_DEVICE_0 \|
Davidroid	0:2887415a46cd	1026	(uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_0);
Davidroid	0:2887415a46cd	1027	break;
Davidroid	0:2887415a46cd	1028	case 1:
Davidroid	0:2887415a46cd	1029	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1030	L6474_TVAL,
Davidroid	0:2887415a46cd	1031	L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1));
Davidroid	0:2887415a46cd	1032	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1033	L6474_T_FAST,
Davidroid	0:2887415a46cd	1034	(uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_1 \|
Davidroid	0:2887415a46cd	1035	(uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_1);
Davidroid	0:2887415a46cd	1036	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1037	L6474_TON_MIN,
Davidroid	0:2887415a46cd	1038	L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_1));
Davidroid	0:2887415a46cd	1039	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1040	L6474_TOFF_MIN,
Davidroid	0:2887415a46cd	1041	L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_1));
Davidroid	0:2887415a46cd	1042	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1043	L6474_OCD_TH,
Davidroid	0:2887415a46cd	1044	L6474_CONF_PARAM_OCD_TH_DEVICE_1);
Davidroid	0:2887415a46cd	1045	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1046	L6474_STEP_MODE,
Davidroid	0:2887415a46cd	1047	(uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_1 \|
Davidroid	0:2887415a46cd	1048	(uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_1);
Davidroid	0:2887415a46cd	1049	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1050	L6474_ALARM_EN,
Davidroid	0:2887415a46cd	1051	L6474_CONF_PARAM_ALARM_EN_DEVICE_1);
Davidroid	0:2887415a46cd	1052	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1053	L6474_CONFIG,
Davidroid	0:2887415a46cd	1054	(uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_1 \|
Davidroid	0:2887415a46cd	1055	(uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_1 \|
Davidroid	0:2887415a46cd	1056	(uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_1 \|
Davidroid	0:2887415a46cd	1057	(uint16_t)L6474_CONF_PARAM_SR_DEVICE_1 \|
Davidroid	0:2887415a46cd	1058	(uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_1);
Davidroid	0:2887415a46cd	1059	break;
Davidroid	0:2887415a46cd	1060	case 2:
Davidroid	0:2887415a46cd	1061	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1062	L6474_TVAL,
Davidroid	0:2887415a46cd	1063	L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2));
Davidroid	0:2887415a46cd	1064	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1065	L6474_T_FAST,
Davidroid	0:2887415a46cd	1066	(uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_2 \|
Davidroid	0:2887415a46cd	1067	(uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_2);
Davidroid	0:2887415a46cd	1068	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1069	L6474_TON_MIN,
Davidroid	0:2887415a46cd	1070	L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_2));
Davidroid	0:2887415a46cd	1071	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1072	L6474_TOFF_MIN,
Davidroid	0:2887415a46cd	1073	L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_2));
Davidroid	0:2887415a46cd	1074	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1075	L6474_OCD_TH,
Davidroid	0:2887415a46cd	1076	L6474_CONF_PARAM_OCD_TH_DEVICE_2);
Davidroid	0:2887415a46cd	1077	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1078	L6474_STEP_MODE,
Davidroid	0:2887415a46cd	1079	(uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_2 \|
Davidroid	0:2887415a46cd	1080	(uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_2);
Davidroid	0:2887415a46cd	1081	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1082	L6474_ALARM_EN,
Davidroid	0:2887415a46cd	1083	L6474_CONF_PARAM_ALARM_EN_DEVICE_2);
Davidroid	0:2887415a46cd	1084	L6474_CmdSetParam(
Davidroid	0:2887415a46cd	1085	L6474_CONFIG,
Davidroid	0:2887415a46cd	1086	(uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_2 \|
Davidroid	0:2887415a46cd	1087	(uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_2 \|
Davidroid	0:2887415a46cd	1088	(uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_2 \|
Davidroid	0:2887415a46cd	1089	(uint16_t)L6474_CONF_PARAM_SR_DEVICE_2 \|
Davidroid	0:2887415a46cd	1090	(uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_2);
Davidroid	0:2887415a46cd	1091	break;
Davidroid	0:2887415a46cd	1092	default: ;
Davidroid	0:2887415a46cd	1093	}
Davidroid	0:2887415a46cd	1094	}
Davidroid	0:2887415a46cd	1095
Davidroid	5:d3c78f12a78d	1096	/**********************************************************
Davidroid	18:2d6ab2b93685	1097	* @brief Sets the registers of the L6474 to initialization values.
Davidroid	18:2d6ab2b93685	1098	* @param init Initialization structure.
Davidroid	18:2d6ab2b93685	1099	* @retval None.
Davidroid	18:2d6ab2b93685	1100	**********************************************************/
Davidroid	18:2d6ab2b93685	1101	void L6474::L6474_SetRegisterToInitializationValues(L6474_InitTypeDef *init)
Davidroid	18:2d6ab2b93685	1102	{
Davidroid	18:2d6ab2b93685	1103	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1104	L6474_ABS_POS,
Davidroid	18:2d6ab2b93685	1105	0
Davidroid	18:2d6ab2b93685	1106	);
Davidroid	18:2d6ab2b93685	1107	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1108	L6474_EL_POS,
Davidroid	18:2d6ab2b93685	1109	0
Davidroid	18:2d6ab2b93685	1110	);
Davidroid	18:2d6ab2b93685	1111	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1112	L6474_MARK,
Davidroid	18:2d6ab2b93685	1113	0
Davidroid	18:2d6ab2b93685	1114	);
Davidroid	18:2d6ab2b93685	1115	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1116	L6474_TVAL,
Davidroid	18:2d6ab2b93685	1117	L6474_Tval_Current_to_Par(init->torque_regulation_current_mA)
Davidroid	18:2d6ab2b93685	1118	);
Davidroid	18:2d6ab2b93685	1119	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1120	L6474_T_FAST,
Davidroid	18:2d6ab2b93685	1121	(uint8_t) init->maximum_fast_decay_time \|
Davidroid	18:2d6ab2b93685	1122	(uint8_t) init->fall_time
Davidroid	18:2d6ab2b93685	1123	);
Davidroid	18:2d6ab2b93685	1124	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1125	L6474_TON_MIN,
Davidroid	18:2d6ab2b93685	1126	L6474_Tmin_Time_to_Par(init->minimum_ON_time_us)
Davidroid	18:2d6ab2b93685	1127	);
Davidroid	18:2d6ab2b93685	1128	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1129	L6474_TOFF_MIN,
Davidroid	18:2d6ab2b93685	1130	L6474_Tmin_Time_to_Par(init->minimum_OFF_time_us)
Davidroid	18:2d6ab2b93685	1131	);
Davidroid	18:2d6ab2b93685	1132	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1133	L6474_OCD_TH,
Davidroid	18:2d6ab2b93685	1134	init->overcurrent_threshold
Davidroid	18:2d6ab2b93685	1135	);
Davidroid	18:2d6ab2b93685	1136	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1137	L6474_STEP_MODE,
Davidroid	18:2d6ab2b93685	1138	(uint8_t) init->step_selection \|
Davidroid	18:2d6ab2b93685	1139	(uint8_t) init->sync_selection
Davidroid	18:2d6ab2b93685	1140	);
Davidroid	18:2d6ab2b93685	1141	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1142	L6474_ALARM_EN,
Davidroid	18:2d6ab2b93685	1143	init->alarm
Davidroid	18:2d6ab2b93685	1144	);
Davidroid	18:2d6ab2b93685	1145	L6474_CmdSetParam(
Davidroid	18:2d6ab2b93685	1146	L6474_CONFIG,
Davidroid	18:2d6ab2b93685	1147	(uint16_t) init->clock \|
Davidroid	18:2d6ab2b93685	1148	(uint16_t) init->torque_regulation_method \|
Davidroid	18:2d6ab2b93685	1149	(uint16_t) init->overcurrent_shutwdown \|
Davidroid	18:2d6ab2b93685	1150	(uint16_t) init->slew_rate \|
Davidroid	18:2d6ab2b93685	1151	(uint16_t) init->target_swicthing_period
Davidroid	18:2d6ab2b93685	1152	);
Davidroid	18:2d6ab2b93685	1153	}
Davidroid	18:2d6ab2b93685	1154
Davidroid	18:2d6ab2b93685	1155	/**********************************************************
Davidroid	0:2887415a46cd	1156	* @brief Sets the parameters of the device to predefined values
Davidroid	0:2887415a46cd	1157	* from l6474_target_config.h
Davidroid	0:2887415a46cd	1158	* @param None
Davidroid	0:2887415a46cd	1159	* @retval None
Davidroid	0:2887415a46cd	1160	**********************************************************/
Davidroid	0:2887415a46cd	1161	void L6474::L6474_SetDeviceParamsToPredefinedValues(void)
Davidroid	0:2887415a46cd	1162	{
Davidroid	18:2d6ab2b93685	1163	switch (device_instance)
Davidroid	0:2887415a46cd	1164	{
Davidroid	0:2887415a46cd	1165	case 0:
Davidroid	18:2d6ab2b93685	1166	device_prm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_0;
Davidroid	18:2d6ab2b93685	1167	device_prm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_0;
Davidroid	18:2d6ab2b93685	1168	device_prm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_0;
Davidroid	18:2d6ab2b93685	1169	device_prm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_0;
Davidroid	0:2887415a46cd	1170	break;
Davidroid	0:2887415a46cd	1171
Davidroid	0:2887415a46cd	1172	case 1:
Davidroid	18:2d6ab2b93685	1173	device_prm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_1;
Davidroid	18:2d6ab2b93685	1174	device_prm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_1;
Davidroid	18:2d6ab2b93685	1175	device_prm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_1;
Davidroid	18:2d6ab2b93685	1176	device_prm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_1;
Davidroid	0:2887415a46cd	1177	break;
Davidroid	0:2887415a46cd	1178
Davidroid	0:2887415a46cd	1179	case 2:
Davidroid	18:2d6ab2b93685	1180	device_prm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_2;
Davidroid	18:2d6ab2b93685	1181	device_prm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_2;
Davidroid	18:2d6ab2b93685	1182	device_prm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_2;
Davidroid	18:2d6ab2b93685	1183	device_prm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_2;
Davidroid	0:2887415a46cd	1184	break;
Davidroid	0:2887415a46cd	1185	}
Davidroid	0:2887415a46cd	1186
Davidroid	18:2d6ab2b93685	1187	device_prm.accu = 0;
Davidroid	18:2d6ab2b93685	1188	device_prm.currentPosition = 0;
Davidroid	18:2d6ab2b93685	1189	device_prm.endAccPos = 0;
Davidroid	18:2d6ab2b93685	1190	device_prm.relativePos = 0;
Davidroid	18:2d6ab2b93685	1191	device_prm.startDecPos = 0;
Davidroid	18:2d6ab2b93685	1192	device_prm.stepsToTake = 0;
Davidroid	18:2d6ab2b93685	1193	device_prm.speed = 0;
Davidroid	18:2d6ab2b93685	1194	device_prm.commandExecuted = NO_CMD;
Davidroid	18:2d6ab2b93685	1195	device_prm.direction = FORWARD;
Davidroid	18:2d6ab2b93685	1196	device_prm.motionState = INACTIVE;
Davidroid	0:2887415a46cd	1197	}
Davidroid	0:2887415a46cd	1198
Davidroid	5:d3c78f12a78d	1199	/**********************************************************
Davidroid	0:2887415a46cd	1200	* @brief Initialises the bridge parameters to start the movement
Davidroid	0:2887415a46cd	1201	* and enable the power bridge
Davidroid	0:2887415a46cd	1202	* @retval None
Davidroid	0:2887415a46cd	1203	**********************************************************/
Davidroid	0:2887415a46cd	1204	void L6474::L6474_StartMovement(void)
Davidroid	0:2887415a46cd	1205	{
Davidroid	0:2887415a46cd	1206	/* Enable L6474 powerstage */
Davidroid	0:2887415a46cd	1207	L6474_CmdEnable();
Davidroid	18:2d6ab2b93685	1208	if (device_prm.endAccPos != 0)
Davidroid	0:2887415a46cd	1209	{
Davidroid	18:2d6ab2b93685	1210	device_prm.motionState = ACCELERATING;
Davidroid	0:2887415a46cd	1211	}
Davidroid	0:2887415a46cd	1212	else
Davidroid	0:2887415a46cd	1213	{
Davidroid	18:2d6ab2b93685	1214	device_prm.motionState = DECELERATING;
Davidroid	0:2887415a46cd	1215	}
Davidroid	18:2d6ab2b93685	1216	device_prm.accu = 0;
Davidroid	18:2d6ab2b93685	1217	device_prm.relativePos = 0;
Davidroid	18:2d6ab2b93685	1218	L6474_ApplySpeed(device_prm.minSpeed);
Davidroid	0:2887415a46cd	1219	}
Davidroid	0:2887415a46cd	1220
Davidroid	5:d3c78f12a78d	1221	/**********************************************************
Davidroid	0:2887415a46cd	1222	* @brief Handles the device state machine at each ste
Davidroid	0:2887415a46cd	1223	* @retval None
Davidroid	0:2887415a46cd	1224	* @note Must only be called by the timer ISR
Davidroid	0:2887415a46cd	1225	**********************************************************/
Davidroid	0:2887415a46cd	1226	void L6474::L6474_StepClockHandler(void)
Davidroid	0:2887415a46cd	1227	{
Davidroid	0:2887415a46cd	1228	/* Set isr flag */
Davidroid	18:2d6ab2b93685	1229	isr_flag = TRUE;
Davidroid	0:2887415a46cd	1230
Davidroid	0:2887415a46cd	1231	/* Incrementation of the relative position */
Davidroid	18:2d6ab2b93685	1232	device_prm.relativePos++;
Davidroid	0:2887415a46cd	1233
Davidroid	18:2d6ab2b93685	1234	switch (device_prm.motionState)
Davidroid	0:2887415a46cd	1235	{
Davidroid	0:2887415a46cd	1236	case ACCELERATING:
Davidroid	0:2887415a46cd	1237	{
Davidroid	18:2d6ab2b93685	1238	uint32_t relPos = device_prm.relativePos;
Davidroid	18:2d6ab2b93685	1239	uint32_t endAccPos = device_prm.endAccPos;
Davidroid	18:2d6ab2b93685	1240	uint16_t speed = device_prm.speed;
Davidroid	18:2d6ab2b93685	1241	uint32_t acc = ((uint32_t)device_prm.acceleration << 16);
Davidroid	0:2887415a46cd	1242
Davidroid	18:2d6ab2b93685	1243	if ((device_prm.commandExecuted == SOFT_STOP_CMD)\|\|
Davidroid	18:2d6ab2b93685	1244	((device_prm.commandExecuted != RUN_CMD)&&
Davidroid	18:2d6ab2b93685	1245	(relPos == device_prm.startDecPos)))
Davidroid	0:2887415a46cd	1246	{
Davidroid	18:2d6ab2b93685	1247	device_prm.motionState = DECELERATING;
Davidroid	18:2d6ab2b93685	1248	device_prm.accu = 0;
Davidroid	0:2887415a46cd	1249	}
Davidroid	18:2d6ab2b93685	1250	else if ((speed >= device_prm.maxSpeed)\|\|
Davidroid	18:2d6ab2b93685	1251	((device_prm.commandExecuted != RUN_CMD)&&
Davidroid	0:2887415a46cd	1252	(relPos == endAccPos)))
Davidroid	0:2887415a46cd	1253	{
Davidroid	18:2d6ab2b93685	1254	device_prm.motionState = STEADY;
Davidroid	0:2887415a46cd	1255	}
Davidroid	0:2887415a46cd	1256	else
Davidroid	0:2887415a46cd	1257	{
Davidroid	0:2887415a46cd	1258	bool speedUpdated = FALSE;
Davidroid	0:2887415a46cd	1259	/* Go on accelerating */
Davidroid	0:2887415a46cd	1260	if (speed == 0) speed =1;
Davidroid	18:2d6ab2b93685	1261	device_prm.accu += acc / speed;
Davidroid	18:2d6ab2b93685	1262	while (device_prm.accu >= (0X10000L))
Davidroid	0:2887415a46cd	1263	{
Davidroid	18:2d6ab2b93685	1264	device_prm.accu -= (0X10000L);
Davidroid	0:2887415a46cd	1265	speed +=1;
Davidroid	0:2887415a46cd	1266	speedUpdated = TRUE;
Davidroid	0:2887415a46cd	1267	}
Davidroid	0:2887415a46cd	1268
Davidroid	0:2887415a46cd	1269	if (speedUpdated)
Davidroid	0:2887415a46cd	1270	{
Davidroid	18:2d6ab2b93685	1271	if (speed > device_prm.maxSpeed)
Davidroid	0:2887415a46cd	1272	{
Davidroid	18:2d6ab2b93685	1273	speed = device_prm.maxSpeed;
Davidroid	0:2887415a46cd	1274	}
Davidroid	18:2d6ab2b93685	1275	device_prm.speed = speed;
Davidroid	18:2d6ab2b93685	1276	L6474_ApplySpeed(device_prm.speed);
Davidroid	0:2887415a46cd	1277	}
Davidroid	0:2887415a46cd	1278	}
Davidroid	0:2887415a46cd	1279	break;
Davidroid	0:2887415a46cd	1280	}
Davidroid	0:2887415a46cd	1281	case STEADY:
Davidroid	0:2887415a46cd	1282	{
Davidroid	18:2d6ab2b93685	1283	uint16_t maxSpeed = device_prm.maxSpeed;
Davidroid	18:2d6ab2b93685	1284	uint32_t relativePos = device_prm.relativePos;
Davidroid	18:2d6ab2b93685	1285	if ((device_prm.commandExecuted == SOFT_STOP_CMD)\|\|
Davidroid	18:2d6ab2b93685	1286	((device_prm.commandExecuted != RUN_CMD)&&
Davidroid	18:2d6ab2b93685	1287	(relativePos >= (device_prm.startDecPos))) \|\|
Davidroid	18:2d6ab2b93685	1288	((device_prm.commandExecuted == RUN_CMD)&&
Davidroid	18:2d6ab2b93685	1289	(device_prm.speed > maxSpeed)))
Davidroid	0:2887415a46cd	1290	{
Davidroid	18:2d6ab2b93685	1291	device_prm.motionState = DECELERATING;
Davidroid	18:2d6ab2b93685	1292	device_prm.accu = 0;
Davidroid	0:2887415a46cd	1293	}
Davidroid	18:2d6ab2b93685	1294	else if ((device_prm.commandExecuted == RUN_CMD)&&
Davidroid	18:2d6ab2b93685	1295	(device_prm.speed < maxSpeed))
Davidroid	0:2887415a46cd	1296	{
Davidroid	18:2d6ab2b93685	1297	device_prm.motionState = ACCELERATING;
Davidroid	18:2d6ab2b93685	1298	device_prm.accu = 0;
Davidroid	0:2887415a46cd	1299	}
Davidroid	0:2887415a46cd	1300	break;
Davidroid	0:2887415a46cd	1301	}
Davidroid	0:2887415a46cd	1302	case DECELERATING:
Davidroid	0:2887415a46cd	1303	{
Davidroid	18:2d6ab2b93685	1304	uint32_t relativePos = device_prm.relativePos;
Davidroid	18:2d6ab2b93685	1305	uint16_t speed = device_prm.speed;
Davidroid	18:2d6ab2b93685	1306	uint32_t deceleration = ((uint32_t)device_prm.deceleration << 16);
Davidroid	18:2d6ab2b93685	1307	if (((device_prm.commandExecuted == SOFT_STOP_CMD)&&(speed <= device_prm.minSpeed))\|\|
Davidroid	18:2d6ab2b93685	1308	((device_prm.commandExecuted != RUN_CMD)&&
Davidroid	18:2d6ab2b93685	1309	(relativePos >= device_prm.stepsToTake)))
Davidroid	0:2887415a46cd	1310	{
Davidroid	0:2887415a46cd	1311	/* Motion process complete */
Davidroid	0:2887415a46cd	1312	L6474_HardStop();
Davidroid	0:2887415a46cd	1313	}
Davidroid	18:2d6ab2b93685	1314	else if ((device_prm.commandExecuted == RUN_CMD)&&
Davidroid	18:2d6ab2b93685	1315	(speed <= device_prm.maxSpeed))
Davidroid	0:2887415a46cd	1316	{
Davidroid	18:2d6ab2b93685	1317	device_prm.motionState = STEADY;
Davidroid	0:2887415a46cd	1318	}
Davidroid	0:2887415a46cd	1319	else
Davidroid	0:2887415a46cd	1320	{
Davidroid	0:2887415a46cd	1321	/* Go on decelerating */
Davidroid	18:2d6ab2b93685	1322	if (speed > device_prm.minSpeed)
Davidroid	0:2887415a46cd	1323	{
Davidroid	0:2887415a46cd	1324	bool speedUpdated = FALSE;
Davidroid	0:2887415a46cd	1325	if (speed == 0) speed =1;
Davidroid	18:2d6ab2b93685	1326	device_prm.accu += deceleration / speed;
Davidroid	18:2d6ab2b93685	1327	while (device_prm.accu >= (0X10000L))
Davidroid	0:2887415a46cd	1328	{
Davidroid	18:2d6ab2b93685	1329	device_prm.accu -= (0X10000L);
Davidroid	0:2887415a46cd	1330	if (speed > 1)
Davidroid	0:2887415a46cd	1331	{
Davidroid	0:2887415a46cd	1332	speed -=1;
Davidroid	0:2887415a46cd	1333	}
Davidroid	0:2887415a46cd	1334	speedUpdated = TRUE;
Davidroid	0:2887415a46cd	1335	}
Davidroid	0:2887415a46cd	1336
Davidroid	0:2887415a46cd	1337	if (speedUpdated)
Davidroid	0:2887415a46cd	1338	{
Davidroid	18:2d6ab2b93685	1339	if (speed < device_prm.minSpeed)
Davidroid	0:2887415a46cd	1340	{
Davidroid	18:2d6ab2b93685	1341	speed = device_prm.minSpeed;
Davidroid	0:2887415a46cd	1342	}
Davidroid	18:2d6ab2b93685	1343	device_prm.speed = speed;
Davidroid	18:2d6ab2b93685	1344	L6474_ApplySpeed(device_prm.speed);
Davidroid	0:2887415a46cd	1345	}
Davidroid	0:2887415a46cd	1346	}
Davidroid	0:2887415a46cd	1347	}
Davidroid	0:2887415a46cd	1348	break;
Davidroid	0:2887415a46cd	1349	}
Davidroid	0:2887415a46cd	1350	default:
Davidroid	0:2887415a46cd	1351	{
Davidroid	0:2887415a46cd	1352	break;
Davidroid	0:2887415a46cd	1353	}
Davidroid	0:2887415a46cd	1354	}
Davidroid	0:2887415a46cd	1355	/* Set isr flag */
Davidroid	18:2d6ab2b93685	1356	isr_flag = FALSE;
Davidroid	18:2d6ab2b93685	1357	}
Davidroid	18:2d6ab2b93685	1358
Davidroid	18:2d6ab2b93685	1359	/**********************************************************
Davidroid	18:2d6ab2b93685	1360	* @brief Converts current in mA to values for TVAL register
Davidroid	18:2d6ab2b93685	1361	* @param[in] current_mA current in mA
Davidroid	18:2d6ab2b93685	1362	* @retval value for TVAL register
Davidroid	18:2d6ab2b93685	1363	**********************************************************/
Davidroid	18:2d6ab2b93685	1364	float L6474::L6474_Tval_Current_to_Par(float current_mA)
Davidroid	18:2d6ab2b93685	1365	{
Davidroid	18:2d6ab2b93685	1366	return ((float)(((current_mA - 31.25f) / 31.25f) + 0.5f));
Davidroid	0:2887415a46cd	1367	}
Davidroid	0:2887415a46cd	1368
Davidroid	5:d3c78f12a78d	1369	/**********************************************************
Davidroid	18:2d6ab2b93685	1370	* @brief Converts values from TVAL register to mA
Davidroid	18:2d6ab2b93685	1371	* @param[in] Tval value from TVAL register
Davidroid	18:2d6ab2b93685	1372	* @retval current in mA
Davidroid	0:2887415a46cd	1373	**********************************************************/
Davidroid	18:2d6ab2b93685	1374	float L6474::L6474_Par_to_Tval_Current(float Tval)
Davidroid	0:2887415a46cd	1375	{
Davidroid	18:2d6ab2b93685	1376	return ((float)((Tval - 0.5f) * 31.25f + 31.25f));
Davidroid	0:2887415a46cd	1377	}
Davidroid	0:2887415a46cd	1378
Davidroid	5:d3c78f12a78d	1379	/**********************************************************
Davidroid	18:2d6ab2b93685	1380	* @brief Convert time in us to values for TON_MIN register
Davidroid	18:2d6ab2b93685	1381	* @param[in] ton_min_us time in us
Davidroid	18:2d6ab2b93685	1382	* @retval value for TON_MIN register
Davidroid	0:2887415a46cd	1383	**********************************************************/
Davidroid	18:2d6ab2b93685	1384	float L6474::L6474_Tmin_Time_to_Par(float ton_min_us)
Davidroid	0:2887415a46cd	1385	{
Davidroid	18:2d6ab2b93685	1386	return ((float)(((ton_min_us - 0.5f) * 2.0f) + 0.5f));
Davidroid	18:2d6ab2b93685	1387	}
Davidroid	18:2d6ab2b93685	1388
Davidroid	18:2d6ab2b93685	1389	/**********************************************************
Davidroid	18:2d6ab2b93685	1390	* @brief Convert values for TON_MIN register to time in us
Davidroid	18:2d6ab2b93685	1391	* @param[in] Tmin value from TON_MIN register
Davidroid	18:2d6ab2b93685	1392	* @retval time in us
Davidroid	18:2d6ab2b93685	1393	**********************************************************/
Davidroid	18:2d6ab2b93685	1394	float L6474::L6474_Par_to_Tmin_Time(float Tmin)
Davidroid	18:2d6ab2b93685	1395	{
Davidroid	18:2d6ab2b93685	1396	return ((float)(((Tmin - 0.5f) / 2.0f) + 0.5f));
Davidroid	0:2887415a46cd	1397	}
Davidroid	0:2887415a46cd	1398
Davidroid	5:d3c78f12a78d	1399	/**********************************************************
Davidroid	0:2887415a46cd	1400	* @brief Write and receive a byte via SPI
Davidroid	0:2887415a46cd	1401	* @param[in] pByteToTransmit pointer to the byte to transmit
Davidroid	0:2887415a46cd	1402	* @param[in] pReceivedByte pointer to the received byte
Davidroid	0:2887415a46cd	1403	* @retval None
Davidroid	0:2887415a46cd	1404	**********************************************************/
Davidroid	0:2887415a46cd	1405	void L6474::L6474_WriteBytes(uint8_t pByteToTransmit, uint8_t pReceivedByte)
Davidroid	0:2887415a46cd	1406	{
Davidroid	0:2887415a46cd	1407	if (L6474_SpiWriteBytes(pByteToTransmit, pReceivedByte) != 0)
Davidroid	0:2887415a46cd	1408	{
Davidroid	0:2887415a46cd	1409	L6474_ErrorHandler(L6474_ERROR_1);
Davidroid	0:2887415a46cd	1410	}
Davidroid	0:2887415a46cd	1411
Davidroid	18:2d6ab2b93685	1412	if (isr_flag)
Davidroid	0:2887415a46cd	1413	{
Davidroid	18:2d6ab2b93685	1414	spi_preemtion_by_isr = TRUE;
Davidroid	0:2887415a46cd	1415	}
Davidroid	0:2887415a46cd	1416	}
Davidroid	0:2887415a46cd	1417
Davidroid	0:2887415a46cd	1418	/********************** (C) COPYRIGHT STMicroelectronics *END OF FILE**/

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	24 May 2017
Imports:	0
Forks:	0
Commits:	27
Dependents:	1
Dependencies:	1
Followers:	1

Components/l6474/l6474_class.cpp@18:2d6ab2b93685, 2016-01-13 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning