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@26:e2049a15fb15, 2017-05-24 (annotated)

Committer:: Arkadi
Date:: Wed May 24 13:49:40 2017 +0000
Revision:: 26:e2049a15fb15
Parent:: 25:9933118b7fc7

Disabled PWM & direction control in library

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

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning