diff --git a/CHANGELOG.md b/CHANGELOG.md index e5684a89..a257960d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,8 @@ Please add a note of your changes below this heading if you make a Pull Request. * Supress startup transients by sampling encoder estimate into position setpoint when entering closed loop control. * Make step dir gpio pins configurable. * Configuration variable `encoder.config.zero_count_on_find_idx`, true by default. Set to false to leave the initial encoder count to be where the axis was at boot. +* Circular position setpoint mode: position setpoints wrapped [0, cpr). Useful for infinite incremental position control. +* Velocity setpoint ramping. Use velocity control mode, and set `controller.vel_ramp_enable` to true. This will ramp `controller.vel_setpoint` towards `controller.vel_ramp_target` at a ramp rate of `controller.config.vel_ramp_rate`. ### Changed * Increased switching frequency from around 8kHz to 24kHz. Control loops still run at 8kHz. @@ -14,6 +16,7 @@ Please add a note of your changes below this heading if you make a Pull Request. ## Fixed * Would get ERROR_CONTROL_DEADLINE_MISSED along with every ERROR_PHASE_RESISTANCE_OUT_OF_RANGE. +* ODrive tool can now run interactive nested scripts with "%run -i script.py" # Releases ## [0.4.6] - 2018-10-07 diff --git a/Firmware/Board/v3/Inc/stm32f4xx_it.h b/Firmware/Board/v3/Inc/stm32f4xx_it.h index bed8a253..784a3333 100644 --- a/Firmware/Board/v3/Inc/stm32f4xx_it.h +++ b/Firmware/Board/v3/Inc/stm32f4xx_it.h @@ -54,8 +54,10 @@ void BusFault_Handler(void); void UsageFault_Handler(void); void DebugMon_Handler(void); void SysTick_Handler(void); +void DMA1_Stream0_IRQHandler(void); void DMA1_Stream2_IRQHandler(void); void DMA1_Stream4_IRQHandler(void); +void DMA1_Stream5_IRQHandler(void); void ADC_IRQHandler(void); void CAN1_TX_IRQHandler(void); void CAN1_RX0_IRQHandler(void); @@ -63,6 +65,7 @@ void CAN1_RX1_IRQHandler(void); void CAN1_SCE_IRQHandler(void); void TIM8_TRG_COM_TIM14_IRQHandler(void); void TIM5_IRQHandler(void); +void SPI3_IRQHandler(void); void UART4_IRQHandler(void); void OTG_FS_IRQHandler(void); diff --git a/Firmware/Board/v3/Odrive.ioc b/Firmware/Board/v3/Odrive.ioc index 9f636781..7a334af7 100644 --- a/Firmware/Board/v3/Odrive.ioc +++ b/Firmware/Board/v3/Odrive.ioc @@ -97,7 +97,29 @@ Dma.ADC1.2.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlign Dma.Request0=UART4_RX Dma.Request1=UART4_TX Dma.Request2=ADC1 -Dma.RequestsNb=3 +Dma.Request3=SPI3_TX +Dma.Request4=SPI3_RX +Dma.RequestsNb=5 +Dma.SPI3_RX.4.Direction=DMA_PERIPH_TO_MEMORY +Dma.SPI3_RX.4.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.SPI3_RX.4.Instance=DMA1_Stream0 +Dma.SPI3_RX.4.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.SPI3_RX.4.MemInc=DMA_MINC_ENABLE +Dma.SPI3_RX.4.Mode=DMA_NORMAL +Dma.SPI3_RX.4.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.SPI3_RX.4.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_RX.4.Priority=DMA_PRIORITY_MEDIUM +Dma.SPI3_RX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode +Dma.SPI3_TX.3.Direction=DMA_MEMORY_TO_PERIPH +Dma.SPI3_TX.3.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.SPI3_TX.3.Instance=DMA1_Stream5 +Dma.SPI3_TX.3.MemDataAlignment=DMA_MDATAALIGN_HALFWORD +Dma.SPI3_TX.3.MemInc=DMA_MINC_ENABLE +Dma.SPI3_TX.3.Mode=DMA_NORMAL +Dma.SPI3_TX.3.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD +Dma.SPI3_TX.3.PeriphInc=DMA_PINC_DISABLE +Dma.SPI3_TX.3.Priority=DMA_PRIORITY_MEDIUM +Dma.SPI3_TX.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode Dma.UART4_RX.0.Direction=DMA_PERIPH_TO_MEMORY Dma.UART4_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE Dma.UART4_RX.0.Instance=DMA1_Stream2 @@ -221,8 +243,10 @@ NVIC.CAN1_RX0_IRQn=true\:6\:0\:true\:false\:true\:true\:true NVIC.CAN1_RX1_IRQn=true\:6\:0\:true\:false\:true\:true\:true NVIC.CAN1_SCE_IRQn=true\:6\:0\:true\:false\:true\:true\:true NVIC.CAN1_TX_IRQn=true\:6\:0\:true\:false\:true\:true\:true +NVIC.DMA1_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:true NVIC.DMA1_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false +NVIC.DMA1_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false NVIC.DMA2_Stream0_IRQn=true\:5\:0\:false\:false\:false\:true\:false NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:true NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true @@ -231,6 +255,7 @@ NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:true NVIC.OTG_FS_IRQn=true\:5\:0\:false\:false\:true\:true\:true NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:true NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 +NVIC.SPI3_IRQn=true\:5\:0\:false\:false\:true\:true\:true NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:true NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:true\:true NVIC.TIM1_UP_TIM10_IRQn=true\:0\:0\:false\:false\:false\:false\:true diff --git a/Firmware/Board/v3/Src/dma.c b/Firmware/Board/v3/Src/dma.c index 3de873ad..813c735a 100644 --- a/Firmware/Board/v3/Src/dma.c +++ b/Firmware/Board/v3/Src/dma.c @@ -71,12 +71,18 @@ void MX_DMA_Init(void) __HAL_RCC_DMA2_CLK_ENABLE(); /* DMA interrupt init */ + /* DMA1_Stream0_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn); /* DMA1_Stream2_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 5, 0); HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn); /* DMA1_Stream4_IRQn interrupt configuration */ HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 5, 0); HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn); + /* DMA1_Stream5_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn); /* DMA2_Stream0_IRQn interrupt configuration */ // Dear STM, no we _don't_ want to fire an interrupt for this DMA // (it's not possible to deselect this in CubeMX) diff --git a/Firmware/Board/v3/Src/spi.c b/Firmware/Board/v3/Src/spi.c index 87b2a018..d66515f9 100644 --- a/Firmware/Board/v3/Src/spi.c +++ b/Firmware/Board/v3/Src/spi.c @@ -51,12 +51,15 @@ #include "spi.h" #include "gpio.h" +#include "dma.h" /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ SPI_HandleTypeDef hspi3; +DMA_HandleTypeDef hdma_spi3_tx; +DMA_HandleTypeDef hdma_spi3_rx; /* SPI3 init function */ void MX_SPI3_Init(void) @@ -105,6 +108,46 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle) GPIO_InitStruct.Alternate = GPIO_AF6_SPI3; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + /* SPI3 DMA Init */ + /* SPI3_TX Init */ + hdma_spi3_tx.Instance = DMA1_Stream5; + hdma_spi3_tx.Init.Channel = DMA_CHANNEL_0; + hdma_spi3_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; + hdma_spi3_tx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_spi3_tx.Init.MemInc = DMA_MINC_ENABLE; + hdma_spi3_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + hdma_spi3_tx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + hdma_spi3_tx.Init.Mode = DMA_NORMAL; + hdma_spi3_tx.Init.Priority = DMA_PRIORITY_MEDIUM; + hdma_spi3_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + if (HAL_DMA_Init(&hdma_spi3_tx) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + __HAL_LINKDMA(spiHandle,hdmatx,hdma_spi3_tx); + + /* SPI3_RX Init */ + hdma_spi3_rx.Instance = DMA1_Stream0; + hdma_spi3_rx.Init.Channel = DMA_CHANNEL_0; + hdma_spi3_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_spi3_rx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_spi3_rx.Init.MemInc = DMA_MINC_ENABLE; + hdma_spi3_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + hdma_spi3_rx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + hdma_spi3_rx.Init.Mode = DMA_NORMAL; + hdma_spi3_rx.Init.Priority = DMA_PRIORITY_MEDIUM; + hdma_spi3_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + if (HAL_DMA_Init(&hdma_spi3_rx) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + __HAL_LINKDMA(spiHandle,hdmarx,hdma_spi3_rx); + + /* SPI3 interrupt Init */ + HAL_NVIC_SetPriority(SPI3_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(SPI3_IRQn); /* USER CODE BEGIN SPI3_MspInit 1 */ /* USER CODE END SPI3_MspInit 1 */ @@ -129,6 +172,12 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef* spiHandle) */ HAL_GPIO_DeInit(GPIOC, GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12); + /* SPI3 DMA DeInit */ + HAL_DMA_DeInit(spiHandle->hdmatx); + HAL_DMA_DeInit(spiHandle->hdmarx); + + /* SPI3 interrupt Deinit */ + HAL_NVIC_DisableIRQ(SPI3_IRQn); /* USER CODE BEGIN SPI3_MspDeInit 1 */ /* USER CODE END SPI3_MspDeInit 1 */ diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 8acc96ee..f0fe1d3e 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -63,6 +63,9 @@ extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc2; extern ADC_HandleTypeDef hadc3; extern CAN_HandleTypeDef hcan1; +extern DMA_HandleTypeDef hdma_spi3_tx; +extern DMA_HandleTypeDef hdma_spi3_rx; +extern SPI_HandleTypeDef hspi3; extern TIM_HandleTypeDef htim5; extern TIM_HandleTypeDef htim8; extern DMA_HandleTypeDef hdma_uart4_rx; @@ -194,6 +197,20 @@ void SysTick_Handler(void) /* please refer to the startup file (startup_stm32f4xx.s). */ /******************************************************************************/ +/** +* @brief This function handles DMA1 stream0 global interrupt. +*/ +void DMA1_Stream0_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream0_IRQn 0 */ + + /* USER CODE END DMA1_Stream0_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_spi3_rx); + /* USER CODE BEGIN DMA1_Stream0_IRQn 1 */ + + /* USER CODE END DMA1_Stream0_IRQn 1 */ +} + /** * @brief This function handles DMA1 stream2 global interrupt. */ @@ -222,6 +239,20 @@ void DMA1_Stream4_IRQHandler(void) /* USER CODE END DMA1_Stream4_IRQn 1 */ } +/** +* @brief This function handles DMA1 stream5 global interrupt. +*/ +void DMA1_Stream5_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream5_IRQn 0 */ + + /* USER CODE END DMA1_Stream5_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_spi3_tx); + /* USER CODE BEGIN DMA1_Stream5_IRQn 1 */ + + /* USER CODE END DMA1_Stream5_IRQn 1 */ +} + /** * @brief This function handles ADC1, ADC2 and ADC3 global interrupts. */ @@ -336,6 +367,20 @@ void TIM5_IRQHandler(void) /* USER CODE END TIM5_IRQn 1 */ } +/** +* @brief This function handles SPI3 global interrupt. +*/ +void SPI3_IRQHandler(void) +{ + /* USER CODE BEGIN SPI3_IRQn 0 */ + + /* USER CODE END SPI3_IRQn 0 */ + HAL_SPI_IRQHandler(&hspi3); + /* USER CODE BEGIN SPI3_IRQn 1 */ + + /* USER CODE END SPI3_IRQn 1 */ +} + /** * @brief This function handles UART4 global interrupt. */ diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 7a11270a..a0912d8f 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -177,7 +177,7 @@ public: // variables exposed on protocol Error_t error_ = ERROR_NONE; bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir - + // updated from config in constructor, and on protocol hook GPIO_TypeDef* step_port_; uint16_t step_pin_; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 8c202ae0..16aef47a 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -118,11 +118,36 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } + // Ramp rate limited velocity setpoint + if (config_.control_mode == CTRL_MODE_VELOCITY_CONTROL && vel_ramp_enable_) { + float max_step_size = current_meas_period * config_.vel_ramp_rate; + float full_step = vel_ramp_target_ - vel_setpoint_; + float step; + if (fabsf(full_step) > max_step_size) { + step = std::copysignf(max_step_size, full_step); + } else { + step = full_step; + } + vel_setpoint_ += step; + } + // Position control // TODO Decide if we want to use encoder or pll position here float vel_des = vel_setpoint_; if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) { - float pos_err = pos_setpoint_ - pos_estimate; + float pos_err; + if (config_.setpoints_in_cpr) { + // TODO this breaks the semantics that estimates come in on the arguments. + // It's probably better to call a get_estimate that will arbitrate (enc vs sensorless) instead. + float cpr = (float)(axis_->encoder_.config_.cpr); + // Keep pos setpoint from drifting + pos_setpoint_ = fmodf_pos(pos_setpoint_, cpr); + // Circular delta + pos_err = pos_setpoint_ - axis_->encoder_.pos_cpr_; + pos_err = wrap_pm(pos_err, 0.5f * cpr); + } else { + pos_err = pos_setpoint_ - pos_estimate; + } vel_des += config_.pos_gain * pos_err; } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index f99f25d4..b39c2890 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -30,6 +30,8 @@ public: float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable + float vel_ramp_rate = 10000.0f; // [(counts/s) / s] + bool setpoints_in_cpr = false; }; Controller(Config_t& config); @@ -82,6 +84,8 @@ public: // float vel_setpoint = 800.0f; float vel_integrator_current_ = 0.0f; // [A] float current_setpoint_ = 0.0f; // [A] + float vel_ramp_target_ = 0.0f; + bool vel_ramp_enable_ = false; uint32_t traj_start_loop_count_ = 0; @@ -93,13 +97,17 @@ public: make_protocol_property("vel_setpoint", &vel_setpoint_), make_protocol_property("vel_integrator_current", &vel_integrator_current_), make_protocol_property("current_setpoint", ¤t_setpoint_), + make_protocol_property("vel_ramp_target", &vel_ramp_target_), + make_protocol_property("vel_ramp_enable", &vel_ramp_enable_), make_protocol_object("config", make_protocol_property("control_mode", &config_.control_mode), make_protocol_property("pos_gain", &config_.pos_gain), make_protocol_property("vel_gain", &config_.vel_gain), make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), make_protocol_property("vel_limit", &config_.vel_limit), - make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance) + make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance), + make_protocol_property("vel_ramp_rate", &config_.vel_ramp_rate), + make_protocol_property("setpoints_in_cpr", &config_.setpoints_in_cpr) ), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, "pos_setpoint", "vel_feed_forward", "current_feed_forward"), diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 5bc256ae..b40b3274 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -667,6 +667,7 @@ void pwm_in_init() { } } +//TODO: These expressions have integer division by 1MHz, so it will be incorrect for clock speeds of not-integer MHz #define TIM_2_5_CLOCK_HZ TIM_APB1_CLOCK_HZ #define PWM_MIN_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse #define PWM_MAX_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward diff --git a/Firmware/fibre/python/fibre/shell.py b/Firmware/fibre/python/fibre/shell.py index 9dd31d7a..d5e24d85 100644 --- a/Firmware/fibre/python/fibre/shell.py +++ b/Firmware/fibre/python/fibre/shell.py @@ -81,6 +81,7 @@ def launch_shell(args, # If IPython is installed, embed IPython shell, otherwise embed regular shell if use_ipython: help = lambda: print_help(args, len(discovered_devices) > 0) # Override help function # pylint: disable=W0612 + locals()['__name__'] = globals()['__name__'] # to fix broken "%run -i script.py" console = IPython.terminal.embed.InteractiveShellEmbed(banner1='') console.runcode = console.run_code # hack to make IPython look like the regular console interact = console