From 8bf3107ca8430283f76978f66686a6c5afb4c0d9 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 17 Oct 2017 18:52:48 -0700 Subject: [PATCH 001/155] Update README.md --- Firmware/README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Firmware/README.md b/Firmware/README.md index 1a0ab8e8..0c526dfb 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -1,5 +1,8 @@ # ODriveFirmware +If you wish to use the latest release, please use the `master` branch (this is the default branch GitHub will present you with). +If you are a developer, you are encouraged to use the `devel` branch, as it contains the latest features. + ### Table of contents From c8514f6087d1b4f3e9c7be9bb73f517b6299febd Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 17 Oct 2017 18:53:20 -0700 Subject: [PATCH 002/155] Update README.md --- Firmware/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/README.md b/Firmware/README.md index 0c526dfb..82e6258b 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -1,6 +1,7 @@ # ODriveFirmware If you wish to use the latest release, please use the `master` branch (this is the default branch GitHub will present you with). + If you are a developer, you are encouraged to use the `devel` branch, as it contains the latest features. ### Table of contents From 32b95d1de92cc6067cc699060face1fdebeee6b4 Mon Sep 17 00:00:00 2001 From: Wetmelon Date: Mon, 6 Nov 2017 00:10:02 -0500 Subject: [PATCH 003/155] Change openocd command to use localhost:3333 --- Firmware/.vscode/tasks.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/.vscode/tasks.json b/Firmware/.vscode/tasks.json index 2d25b89a..6bed3fcf 100644 --- a/Firmware/.vscode/tasks.json +++ b/Firmware/.vscode/tasks.json @@ -24,8 +24,8 @@ { "taskName": "openocd", "type": "shell", - "command": "openocd -f \"interface/stlink-v2.cfg\" -f \"target/stm32f4x_stlink.cfg\" -c \"gdb_port pipe; log_output openocd.log\"", + "command": "openocd -f \"interface/stlink-v2.cfg\" -f \"target/stm32f4x_stlink.cfg\" -c \"gdb_port 3333; log_output openocd.log\"", "problemMatcher": [] } ] -} \ No newline at end of file +} From e8b7deac5a251edee22b8ffe07fb13155aa39be8 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 01:30:13 -0500 Subject: [PATCH 004/155] Make _write wait for CDC transmission to finish --- Firmware/Src/syscalls.c | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index 3cd2a073..1b8e7589 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -5,7 +5,9 @@ ****************************************************************************** */ -#include +#include +#include +#include #include #include #include @@ -19,6 +21,8 @@ #define UART_TX_BUFFER_SIZE 64 static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; +extern PCD_HandleTypeDef hpcd_USB_OTG_FS; +extern USBD_HandleTypeDef hUsbDeviceFS; int _write(int file, char *data, int len) { //number of bytes written @@ -49,5 +53,15 @@ int _write(int file, char *data, int len) { } break; } + // Wait for transmission to complete + USBD_CDC_HandleTypeDef* hcdc = (USBD_CDC_HandleTypeDef*)hUsbDeviceFS.pClassData; + while (hcdc->TxState != 0) { + osSemaphoreWait(sem_usb_irq, 0); + // We have a new incoming USB transmission: handle it + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + // Let the irq (OTG_FS_IRQHandler) fire again. + HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + } + return written; } From c0aa7c1157ca5c6d9ad2670541bc552a08da7274 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 01:47:40 -0500 Subject: [PATCH 005/155] Make UART block until transmission completes --- Firmware/Src/syscalls.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index 1b8e7589..183fa830 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -47,6 +47,9 @@ int _write(int file, char *data, int len) { // Start DMA background trasnfer HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); } break; + while (huart4.gState != HAL_UART_STATE_READY) { + // Do nothing + } default: { written = 0; From 2bb18056572e683a6cd93b733beb4af5f2c3237f Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 01:48:33 -0500 Subject: [PATCH 006/155] Move USB CDC block to case statement + formatting --- Firmware/Src/syscalls.c | 80 ++++++++++++++++++++--------------------- 1 file changed, 39 insertions(+), 41 deletions(-) diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index 183fa830..cd9d449d 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -5,13 +5,12 @@ ****************************************************************************** */ -#include #include -#include -#include -#include #include - +#include +#include +#include +#include //int _read(int file, char *data, int len) {} //int _close(int file) {} @@ -24,47 +23,46 @@ static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; extern PCD_HandleTypeDef hpcd_USB_OTG_FS; extern USBD_HandleTypeDef hUsbDeviceFS; -int _write(int file, char *data, int len) { - //number of bytes written - int written = 0; - switch (serial_printf_select) { +int _write(int file, char* data, int len) { + //number of bytes written + int written = 0; + switch (serial_printf_select) { + case SERIAL_PRINTF_IS_USB: { + // transmit over CDC + uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); + written = (status == USBD_OK) ? len : 0; - case SERIAL_PRINTF_IS_USB: { - // transmit over CDC - uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); - written = (status == USBD_OK) ? len : 0; - } break; + // Wait for transmission to complete + USBD_CDC_HandleTypeDef* hcdc = (USBD_CDC_HandleTypeDef*)hUsbDeviceFS.pClassData; + while (hcdc->TxState != 0) { + osSemaphoreWait(sem_usb_irq, 0); + // We have a new incoming USB transmission: handle it + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + // Let the irq (OTG_FS_IRQHandler) fire again. + HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + } + } break; - case SERIAL_PRINTF_IS_UART: { - //Check length - if (len > UART_TX_BUFFER_SIZE) - return 0; - // Check if transfer is already ongoing - if(huart4.gState != HAL_UART_STATE_READY) - return 0; - // memcpy data into uart_tx_buf - memcpy(uart_tx_buf, data, len); - // Start DMA background trasnfer - HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); - } break; + case SERIAL_PRINTF_IS_UART: { + //Check length + if (len > UART_TX_BUFFER_SIZE) + return 0; + // Check if transfer is already ongoing + if (huart4.gState != HAL_UART_STATE_READY) + return 0; + // memcpy data into uart_tx_buf + memcpy(uart_tx_buf, data, len); + // Start DMA background trasnfer + HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); while (huart4.gState != HAL_UART_STATE_READY) { // Do nothing } + } break; - default: { - written = 0; - } break; - } + default: { + written = 0; + } break; + } - // Wait for transmission to complete - USBD_CDC_HandleTypeDef* hcdc = (USBD_CDC_HandleTypeDef*)hUsbDeviceFS.pClassData; - while (hcdc->TxState != 0) { - osSemaphoreWait(sem_usb_irq, 0); - // We have a new incoming USB transmission: handle it - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - // Let the irq (OTG_FS_IRQHandler) fire again. - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); - } - - return written; + return written; } From 8bd338832e4380cec82f683203c5e34433347205 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 8 Nov 2017 01:22:59 -0800 Subject: [PATCH 007/155] remove unused changelog entry --- Firmware/CHANGELOG.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 4e0f1196..7dea17c9 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,10 +1,9 @@ ## [0.2.0] - UNRELEASED ### Added -* UART feature +* UART communication * Setting to select UART or Step/dir on GIPIO 1,2 * Basic Anti-cogging -### Changed ## [0.1.0] - 2017-08-26 ### Added From 1a1f1a1d93432343db5928e18ac5600ec700ddad Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 05:38:14 -0500 Subject: [PATCH 008/155] Implement new thread for USB IRQ pump --- Firmware/Inc/freertos_vars.h | 3 +++ Firmware/MotorControl/commands.c | 46 +++++++++++++++++++++++--------- Firmware/MotorControl/commands.h | 3 +++ Firmware/Src/freertos.c | 20 ++++++++++++-- Firmware/Src/syscalls.c | 19 +++++++------ Firmware/Src/usbd_cdc_if.c | 7 +++-- 6 files changed, 70 insertions(+), 28 deletions(-) diff --git a/Firmware/Inc/freertos_vars.h b/Firmware/Inc/freertos_vars.h index fd0bf85d..b0d73b35 100644 --- a/Firmware/Inc/freertos_vars.h +++ b/Firmware/Inc/freertos_vars.h @@ -4,10 +4,13 @@ // List of semaphore osSemaphoreId sem_usb_irq; +osSemaphoreId sem_uart_dma; +osSemaphoreId sem_usb_rx; // List of threads osThreadId thread_motor_0; osThreadId thread_motor_1; osThreadId thread_cmd_parse; +osThreadId thread_usb_pump; #endif /* __FREERTOS_H */ \ No newline at end of file diff --git a/Firmware/MotorControl/commands.c b/Firmware/MotorControl/commands.c index 0c01ccd9..fa5425e1 100644 --- a/Firmware/MotorControl/commands.c +++ b/Firmware/MotorControl/commands.c @@ -4,6 +4,7 @@ #include #include #include +#include extern PCD_HandleTypeDef hpcd_USB_OTG_FS; @@ -21,6 +22,10 @@ SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_NONE; static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx // static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir +static uint8_t usb_buf[64]; +static uint32_t usb_len; +extern USBD_HandleTypeDef hUsbDeviceFS; + // variables exposed to usb/serial interface via set/get/monitor // Note: this will be depricated soon static float* const exposed_floats[] = { @@ -163,6 +168,10 @@ void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface if (numscan == 2 && motor_number < num_motors) { set_current_setpoint(&motors[motor_number], current_feed_forward); } + } else if(buffer[0] == 'e'){ + int val = printf("Test 1\n"); + int val2 = printf("test 2\n"); + val = 0; } else if (buffer[0] == 'g') { // GET // g <0:float,1:int,2:bool,3:uint16> index int type = 0; @@ -335,21 +344,34 @@ void cmd_parse_thread(void const * argument) { } } } - // When we reach here, we are out of immediate characters to fetch out of UART buffer - // Now we check if there is any USB processing to do: we wait for up to 1 ms, - // before going back to checking UART again. - int USB_check_timeout = 1; - // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) - osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, USB_check_timeout); - if (semaphore_status == osOK) { - // We have a new incoming USB transmission: handle it - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - // Let the irq (OTG_FS_IRQHandler) fire again. - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + osStatus sem_stat = osSemaphoreWait(sem_usb_rx, 1); + if(sem_stat == osOK){ + motor_parse_cmd(usb_buf, usb_len, SERIAL_PRINTF_IS_USB); + USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet } } while (!reset_read_state); } - // If we get here, then this task is done vTaskDelete(osThreadGetId()); +} + +// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the +// incoming USB data +void set_cmd_buffer(const uint8_t *buf, uint32_t len) { + memcpy(usb_buf, buf, len); + usb_len = len; +} + +void usb_update_thread() { + for (;;) { + // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) + osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); + if (semaphore_status == osOK) { + // We have a new incoming USB transmission: handle it + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + // Let the irq (OTG_FS_IRQHandler) fire again. + HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + } + } + vTaskDelete(osThreadGetId()); } \ No newline at end of file diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 1483af9c..80d1b77a 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -27,4 +27,7 @@ void init_communication(); void cmd_parse_thread(void const * argument); void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface); +void set_cmd_buffer(const uint8_t *buf, uint32_t len); +void usb_update_thread(); + #endif /* COMMANDS_H */ diff --git a/Firmware/Src/freertos.c b/Firmware/Src/freertos.c index da369f40..49d17383 100644 --- a/Firmware/Src/freertos.c +++ b/Firmware/Src/freertos.c @@ -88,10 +88,22 @@ void MX_FREERTOS_Init(void) { /* USER CODE END RTOS_MUTEX */ /* USER CODE BEGIN RTOS_SEMAPHORES */ - // Init usb irq binary semaphore, and start with no tolkens by removing the starting one. + // Init usb irq binary semaphore, and start with no tokens by removing the starting one. osSemaphoreDef(sem_usb_irq); sem_usb_irq = osSemaphoreCreate(osSemaphore(sem_usb_irq), 1); osSemaphoreWait(sem_usb_irq, 0); + + // Create a semaphore for UART DMA and remove a token + osSemaphoreDef(sem_uart_dma); + sem_uart_dma = osSemaphoreCreate(osSemaphore(sem_uart_dma), 1); + osSemaphoreWait(sem_uart_dma, 0); + + // Create a semaphore for USB RX + osSemaphoreDef(sem_usb_rx); + sem_usb_rx = osSemaphoreCreate(osSemaphore(sem_usb_rx), 1); + osSemaphoreWait(sem_usb_irq, 0); // Remove a token. + + /* USER CODE END RTOS_SEMAPHORES */ /* USER CODE BEGIN RTOS_TIMERS */ @@ -132,10 +144,14 @@ void StartDefaultTask(void const * argument) thread_motor_0 = osThreadCreate(osThread(task_motor_0), &motors[0]); thread_motor_1 = osThreadCreate(osThread(task_motor_1), &motors[1]); - // Start USB command handling thread + // Start command handling thread osThreadDef(task_cmd_parse, cmd_parse_thread, osPriorityNormal, 0, 512); thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); + thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); + //If we get to here, then the default task is done. vTaskDelete(defaultTaskHandle); diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index cd9d449d..cbe72817 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -35,11 +35,7 @@ int _write(int file, char* data, int len) { // Wait for transmission to complete USBD_CDC_HandleTypeDef* hcdc = (USBD_CDC_HandleTypeDef*)hUsbDeviceFS.pClassData; while (hcdc->TxState != 0) { - osSemaphoreWait(sem_usb_irq, 0); - // We have a new incoming USB transmission: handle it - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - // Let the irq (OTG_FS_IRQHandler) fire again. - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + // Do nothing } } break; @@ -48,15 +44,14 @@ int _write(int file, char* data, int len) { if (len > UART_TX_BUFFER_SIZE) return 0; // Check if transfer is already ongoing - if (huart4.gState != HAL_UART_STATE_READY) - return 0; + //if (huart4.gState != HAL_UART_STATE_READY) + //return 0; // memcpy data into uart_tx_buf memcpy(uart_tx_buf, data, len); // Start DMA background trasnfer HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); - while (huart4.gState != HAL_UART_STATE_READY) { - // Do nothing - } + // Wait for the transmission to complete + osSemaphoreWait(sem_uart_dma, osWaitForever); } break; default: { @@ -66,3 +61,7 @@ int _write(int file, char* data, int len) { return written; } + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { + osSemaphoreRelease(sem_uart_dma); +} \ No newline at end of file diff --git a/Firmware/Src/usbd_cdc_if.c b/Firmware/Src/usbd_cdc_if.c index 8f6b7317..475c969e 100644 --- a/Firmware/Src/usbd_cdc_if.c +++ b/Firmware/Src/usbd_cdc_if.c @@ -51,6 +51,7 @@ /* USER CODE BEGIN INCLUDE */ #include "utils.h" #include "commands.h" +#include /* USER CODE END INCLUDE */ /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY @@ -272,10 +273,8 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len) int modified_len = MACRO_MIN(*Len+1, APP_RX_DATA_SIZE); Buf[modified_len-1] = 0; - motor_parse_cmd(Buf, modified_len, SERIAL_PRINTF_IS_USB); - - // Allow next packet - USBD_CDC_ReceivePacket(&hUsbDeviceFS); + set_cmd_buffer(Buf, modified_len); + osSemaphoreRelease(sem_usb_rx); return (USBD_OK); /* USER CODE END 6 */ From 7e99b06ad1f3afc6ffeee40a848baceb947685c4 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 05:39:15 -0500 Subject: [PATCH 009/155] Remove unused extern near _write --- Firmware/Src/syscalls.c | 1 - 1 file changed, 1 deletion(-) diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index cbe72817..12624fb7 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -20,7 +20,6 @@ #define UART_TX_BUFFER_SIZE 64 static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; extern USBD_HandleTypeDef hUsbDeviceFS; int _write(int file, char* data, int len) { From 29ba915b54e694dfe6674a73fe08ae0f21810a4d Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 17:39:33 -0500 Subject: [PATCH 010/155] Modify USB TX to use a semaphore from usb_cdc --- Firmware/Inc/freertos_vars.h | 1 + .../STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c | 4 +++- Firmware/Src/freertos.c | 4 ++++ Firmware/Src/syscalls.c | 10 ++-------- 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/Firmware/Inc/freertos_vars.h b/Firmware/Inc/freertos_vars.h index b0d73b35..9a08d016 100644 --- a/Firmware/Inc/freertos_vars.h +++ b/Firmware/Inc/freertos_vars.h @@ -6,6 +6,7 @@ osSemaphoreId sem_usb_irq; osSemaphoreId sem_uart_dma; osSemaphoreId sem_usb_rx; +osSemaphoreId sem_usb_tx; // List of threads osThreadId thread_motor_0; diff --git a/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c b/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c index b2ca5f16..345b0b0f 100644 --- a/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +++ b/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c @@ -62,6 +62,8 @@ #include "usbd_cdc.h" #include "usbd_desc.h" #include "usbd_ctlreq.h" +#include +#include /** @addtogroup STM32_USB_DEVICE_LIBRARY @@ -669,7 +671,7 @@ static uint8_t USBD_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum) { hcdc->TxState = 0; - + osSemaphoreRelease(sem_usb_tx); return USBD_OK; } else diff --git a/Firmware/Src/freertos.c b/Firmware/Src/freertos.c index 49d17383..1ebf93a0 100644 --- a/Firmware/Src/freertos.c +++ b/Firmware/Src/freertos.c @@ -103,6 +103,10 @@ void MX_FREERTOS_Init(void) { sem_usb_rx = osSemaphoreCreate(osSemaphore(sem_usb_rx), 1); osSemaphoreWait(sem_usb_irq, 0); // Remove a token. + // Create a semaphore for USB RX + osSemaphoreDef(sem_usb_tx); + sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); + osSemaphoreWait(sem_usb_tx, 0); // Remove a token. /* USER CODE END RTOS_SEMAPHORES */ diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index 12624fb7..5c92c709 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -20,7 +20,6 @@ #define UART_TX_BUFFER_SIZE 64 static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; -extern USBD_HandleTypeDef hUsbDeviceFS; int _write(int file, char* data, int len) { //number of bytes written @@ -32,23 +31,18 @@ int _write(int file, char* data, int len) { written = (status == USBD_OK) ? len : 0; // Wait for transmission to complete - USBD_CDC_HandleTypeDef* hcdc = (USBD_CDC_HandleTypeDef*)hUsbDeviceFS.pClassData; - while (hcdc->TxState != 0) { - // Do nothing - } + osSemaphoreWait(sem_usb_tx, osWaitForever); } break; case SERIAL_PRINTF_IS_UART: { //Check length if (len > UART_TX_BUFFER_SIZE) return 0; - // Check if transfer is already ongoing - //if (huart4.gState != HAL_UART_STATE_READY) - //return 0; // memcpy data into uart_tx_buf memcpy(uart_tx_buf, data, len); // Start DMA background trasnfer HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); + // Wait for the transmission to complete osSemaphoreWait(sem_uart_dma, osWaitForever); } break; From 8bdd4b9161a1cc2901c086cf5926411f301bb6d6 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 17:45:48 -0500 Subject: [PATCH 011/155] Fix formatting in freertos.c --- Firmware/Src/freertos.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/Src/freertos.c b/Firmware/Src/freertos.c index 1ebf93a0..d91e3ae0 100644 --- a/Firmware/Src/freertos.c +++ b/Firmware/Src/freertos.c @@ -103,10 +103,10 @@ void MX_FREERTOS_Init(void) { sem_usb_rx = osSemaphoreCreate(osSemaphore(sem_usb_rx), 1); osSemaphoreWait(sem_usb_irq, 0); // Remove a token. - // Create a semaphore for USB RX - osSemaphoreDef(sem_usb_tx); - sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); - osSemaphoreWait(sem_usb_tx, 0); // Remove a token. + // Create a semaphore for USB RX + osSemaphoreDef(sem_usb_tx); + sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); + osSemaphoreWait(sem_usb_tx, 0); // Remove a token. /* USER CODE END RTOS_SEMAPHORES */ From ee6c1cebb4cf5091cf0b5d9a0fa74b87d613ac41 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 19:39:42 -0500 Subject: [PATCH 012/155] Modify the _write lock to wait on resource instead of xfer --- Firmware/Src/syscalls.c | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index 5c92c709..ee9da08f 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -26,25 +26,24 @@ int _write(int file, char* data, int len) { int written = 0; switch (serial_printf_select) { case SERIAL_PRINTF_IS_USB: { - // transmit over CDC - uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); - written = (status == USBD_OK) ? len : 0; - - // Wait for transmission to complete - osSemaphoreWait(sem_usb_tx, osWaitForever); + // Wait for the interface to be available + osStatus sem_stat = osSemaphoreWait(sem_usb_tx, osWaitForever); + if (sem_stat == osOK) { + uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); // transmit over CDC + written = (status == USBD_OK) ? len : 0; + } } break; case SERIAL_PRINTF_IS_UART: { //Check length if (len > UART_TX_BUFFER_SIZE) return 0; - // memcpy data into uart_tx_buf - memcpy(uart_tx_buf, data, len); - // Start DMA background trasnfer - HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); - - // Wait for the transmission to complete - osSemaphoreWait(sem_uart_dma, osWaitForever); + // Wait for the interface to be available + osStatus sem_stat = osSemaphoreWait(sem_uart_dma, osWaitForever); + if (sem_stat == osOK) { + memcpy(uart_tx_buf, data, len); // memcpy data into uart_tx_buf + HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); // Start DMA background transfer + } } break; default: { From 81513a97b2e6164bf5a7ed9770031264534305d1 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 19:40:31 -0500 Subject: [PATCH 013/155] Flip the default tx signal value --- Firmware/Src/freertos.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/Firmware/Src/freertos.c b/Firmware/Src/freertos.c index d91e3ae0..e80163b2 100644 --- a/Firmware/Src/freertos.c +++ b/Firmware/Src/freertos.c @@ -96,7 +96,6 @@ void MX_FREERTOS_Init(void) { // Create a semaphore for UART DMA and remove a token osSemaphoreDef(sem_uart_dma); sem_uart_dma = osSemaphoreCreate(osSemaphore(sem_uart_dma), 1); - osSemaphoreWait(sem_uart_dma, 0); // Create a semaphore for USB RX osSemaphoreDef(sem_usb_rx); @@ -106,7 +105,6 @@ void MX_FREERTOS_Init(void) { // Create a semaphore for USB RX osSemaphoreDef(sem_usb_tx); sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); - osSemaphoreWait(sem_usb_tx, 0); // Remove a token. /* USER CODE END RTOS_SEMAPHORES */ From 17375b41d8a550900bfbf9181fb0343ac1da6406 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 19:40:52 -0500 Subject: [PATCH 014/155] Remove memcpy from set_cmd_buffer --- Firmware/MotorControl/commands.c | 9 +++++---- Firmware/MotorControl/commands.h | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/commands.c b/Firmware/MotorControl/commands.c index fa5425e1..2b8c1e4e 100644 --- a/Firmware/MotorControl/commands.c +++ b/Firmware/MotorControl/commands.c @@ -22,7 +22,7 @@ SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_NONE; static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx // static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir -static uint8_t usb_buf[64]; +static uint8_t* usb_buf; static uint32_t usb_len; extern USBD_HandleTypeDef hUsbDeviceFS; @@ -344,7 +344,8 @@ void cmd_parse_thread(void const * argument) { } } } - osStatus sem_stat = osSemaphoreWait(sem_usb_rx, 1); + // Check if there is USB processing to do. + osStatus sem_stat = osSemaphoreWait(sem_usb_rx, 0); if(sem_stat == osOK){ motor_parse_cmd(usb_buf, usb_len, SERIAL_PRINTF_IS_USB); USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet @@ -357,8 +358,8 @@ void cmd_parse_thread(void const * argument) { // Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the // incoming USB data -void set_cmd_buffer(const uint8_t *buf, uint32_t len) { - memcpy(usb_buf, buf, len); +void set_cmd_buffer(uint8_t *buf, uint32_t len) { + usb_buf = buf; usb_len = len; } diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 80d1b77a..1cb5ad31 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -27,7 +27,7 @@ void init_communication(); void cmd_parse_thread(void const * argument); void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface); -void set_cmd_buffer(const uint8_t *buf, uint32_t len); +void set_cmd_buffer(uint8_t *buf, uint32_t len); void usb_update_thread(); #endif /* COMMANDS_H */ From 70b8b4799c7e8b4169783d5ad5e511a96a8f084d Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 20:12:46 -0500 Subject: [PATCH 015/155] Fix timeouts --- Firmware/MotorControl/commands.c | 11 +++++++---- Firmware/Src/syscalls.c | 6 ++++-- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/commands.c b/Firmware/MotorControl/commands.c index 2b8c1e4e..76006a54 100644 --- a/Firmware/MotorControl/commands.c +++ b/Firmware/MotorControl/commands.c @@ -344,11 +344,14 @@ void cmd_parse_thread(void const * argument) { } } } - // Check if there is USB processing to do. - osStatus sem_stat = osSemaphoreWait(sem_usb_rx, 0); - if(sem_stat == osOK){ + // When we reach here, we are out of immediate characters to fetch out of UART buffer + // Now we check if there is any USB processing to do: we wait for up to 1 ms, + // before going back to checking UART again. + const uint32_t usb_check_timeout = 1; // ms + osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); + if (sem_stat == osOK) { motor_parse_cmd(usb_buf, usb_len, SERIAL_PRINTF_IS_USB); - USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet + USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet } } while (!reset_read_state); } diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index ee9da08f..8e422c7b 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -27,7 +27,8 @@ int _write(int file, char* data, int len) { switch (serial_printf_select) { case SERIAL_PRINTF_IS_USB: { // Wait for the interface to be available - osStatus sem_stat = osSemaphoreWait(sem_usb_tx, osWaitForever); + const uint32_t usb_tx_timeout = 100; // ms + osStatus sem_stat = osSemaphoreWait(sem_usb_tx, usb_tx_timeout); if (sem_stat == osOK) { uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); // transmit over CDC written = (status == USBD_OK) ? len : 0; @@ -39,7 +40,8 @@ int _write(int file, char* data, int len) { if (len > UART_TX_BUFFER_SIZE) return 0; // Wait for the interface to be available - osStatus sem_stat = osSemaphoreWait(sem_uart_dma, osWaitForever); + const uint32_t uart_tx_timeout = 100; // ms + osStatus sem_stat = osSemaphoreWait(sem_uart_dma, uart_tx_timeout); if (sem_stat == osOK) { memcpy(uart_tx_buf, data, len); // memcpy data into uart_tx_buf HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); // Start DMA background transfer From 4425cf6b0e762832d3361e38a4696035a00f4693 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 8 Nov 2017 17:23:41 -0800 Subject: [PATCH 016/155] Update syscalls.c --- Firmware/Src/syscalls.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/Firmware/Src/syscalls.c b/Firmware/Src/syscalls.c index 8e422c7b..0f850cda 100644 --- a/Firmware/Src/syscalls.c +++ b/Firmware/Src/syscalls.c @@ -26,26 +26,28 @@ int _write(int file, char* data, int len) { int written = 0; switch (serial_printf_select) { case SERIAL_PRINTF_IS_USB: { - // Wait for the interface to be available + // Wait on semaphore for the interface to be available + // Note that the USB driver will release the interface again when the TX completes const uint32_t usb_tx_timeout = 100; // ms osStatus sem_stat = osSemaphoreWait(sem_usb_tx, usb_tx_timeout); if (sem_stat == osOK) { uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); // transmit over CDC written = (status == USBD_OK) ? len : 0; - } + } // If the semaphore times out, we simply leave "written" as 0 } break; case SERIAL_PRINTF_IS_UART: { //Check length if (len > UART_TX_BUFFER_SIZE) return 0; - // Wait for the interface to be available + // Wait on semaphore for the interface to be available + // Note that HAL_UART_TxCpltCallback will release the interface again when the TX completes const uint32_t uart_tx_timeout = 100; // ms osStatus sem_stat = osSemaphoreWait(sem_uart_dma, uart_tx_timeout); if (sem_stat == osOK) { memcpy(uart_tx_buf, data, len); // memcpy data into uart_tx_buf HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); // Start DMA background transfer - } + } // If the semaphore times out, we simply leave "written" as 0 } break; default: { @@ -58,4 +60,4 @@ int _write(int file, char* data, int len) { void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { osSemaphoreRelease(sem_uart_dma); -} \ No newline at end of file +} From a935c2645555155074c15e37f66ae1647b0aaa04 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Nov 2017 21:46:07 -0500 Subject: [PATCH 017/155] Add UUID access, echo cmd --- Firmware/MotorControl/commands.c | 11 ++++-- Firmware/MotorControl/utils.h | 66 +++++++++++++++++++++++++++++++- tools/test_communication.py | 4 +- 3 files changed, 73 insertions(+), 8 deletions(-) diff --git a/Firmware/MotorControl/commands.c b/Firmware/MotorControl/commands.c index 76006a54..b3d5e4e0 100644 --- a/Firmware/MotorControl/commands.c +++ b/Firmware/MotorControl/commands.c @@ -5,6 +5,7 @@ #include #include #include +#include extern PCD_HandleTypeDef hpcd_USB_OTG_FS; @@ -168,10 +169,12 @@ void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface if (numscan == 2 && motor_number < num_motors) { set_current_setpoint(&motors[motor_number], current_feed_forward); } - } else if(buffer[0] == 'e'){ - int val = printf("Test 1\n"); - int val2 = printf("test 2\n"); - val = 0; + } else if(buffer[0] == 'e'){ // Generic ECHO command for testing + // Retrieves the device signature, revision, flash size, and UUID + printf("Signature: %#x\n", STM_ID_GetSignature()); + printf("Revision: %#x\n", STM_ID_GetRevision()); + printf("Flash Size: %#x KiB\n", STM_ID_GetFlashSize()); + printf("UUID: 0x%lx%lx%lx\n", STM_ID_GetUUID(2), STM_ID_GetUUID(1), STM_ID_GetUUID(0)); } else if (buffer[0] == 'g') { // GET // g <0:float,1:int,2:bool,3:uint16> index int type = 0; diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 7170b8ca..9f995c7b 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -2,6 +2,68 @@ #ifndef __UTILS_H #define __UTILS_H +/** + * @brief Unique ID register address location + */ +#define ID_UNIQUE_ADDRESS (0x1FFF7A10) + +/** + * @brief Flash size register address + */ +#define ID_FLASH_ADDRESS (0x1FFF7A22) + +/** + * @brief Device ID register address + */ +#define ID_DBGMCU_IDCODE (0xE0042000) + +/** + * "Returns" the device signature + * + * Possible returns: + * - 0x0413: STM32F405xx/07xx and STM32F415xx/17xx) + * - 0x0419: STM32F42xxx and STM32F43xxx + * - 0x0423: STM32F401xB/C + * - 0x0433: STM32F401xD/E + * - 0x0431: STM32F411xC/E + * + * Returned data is in 16-bit mode, but only bits 11:0 are valid, bits 15:12 are always 0. + * Defined as macro + */ +#define STM_ID_GetSignature() ((*(uint16_t *)(ID_DBGMCU_IDCODE)) & 0x0FFF) + +/** + * "Returns" the device revision + * + * Revisions possible: + * - 0x1000: Revision A + * - 0x1001: Revision Z + * - 0x1003: Revision Y + * - 0x1007: Revision 1 + * - 0x2001: Revision 3 + * + * Returned data is in 16-bit mode. + */ +#define STM_ID_GetRevision() (*(uint16_t *)(ID_DBGMCU_IDCODE + 2)) + +/** +* "Returns" the Flash size +* +* Returned data is in 16-bit mode, returned value is flash size in kB (kilo bytes). +*/ +#define STM_ID_GetFlashSize() (*(uint16_t *)(ID_FLASH_ADDRESS)) + +/** + * "Returns" the given 32-bit value of the UUID. + * + * Parameters: + * - uint8_t x: + * Value between 0 and 2, corresponding to 4-bytes you want to read from 96bits (12bytes) + * + * Returned data is 32-bit + */ +#define STM_ID_GetUUID(x) ((x >= 0 && x < 3) ? (*(uint32_t *)(ID_UNIQUE_ADDRESS + 4 * (x))) : 0) + #ifndef M_PI #define M_PI 3.14159265358979323846f #endif @@ -13,11 +75,11 @@ // as per the magnitude invariant clarke transform // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2 // Returns 0 on success, and -1 if the input was out of range -int SVM(float alpha, float beta, float* tA, float* tB, float* tC); +int SVM(float alpha, float beta, float *tA, float *tB, float *tC); //beware of inserting large angles! float wrap_pm_pi(float theta); float fast_atan2(float y, float x); int mod(int dividend, int divisor); -#endif //__UTILS_H +#endif //__UTILS_H diff --git a/tools/test_communication.py b/tools/test_communication.py index 0a24fef1..32d989bb 100644 --- a/tools/test_communication.py +++ b/tools/test_communication.py @@ -39,7 +39,7 @@ def main(args): while running: time.sleep(0.1) try: - command = input("Enter ODrive command:\n") + command = input("\r\nEnter ODrive command:\n") if 'q' in command: running = False sys.exit() @@ -52,7 +52,7 @@ def receive_thread(dev): global ready while running: - time.sleep(0.1) + time.sleep(0.001) try: message = dev.receive(dev.receive_max()) message_ascii = bytes(message).decode('ascii') From 70f1a566313955e426f108029182f72f57b6154d Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 8 Nov 2017 21:13:33 -0800 Subject: [PATCH 018/155] move stars in util.h --- Firmware/MotorControl/utils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 9f995c7b..542b7dd7 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -75,7 +75,7 @@ // as per the magnitude invariant clarke transform // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2 // Returns 0 on success, and -1 if the input was out of range -int SVM(float alpha, float beta, float *tA, float *tB, float *tC); +int SVM(float alpha, float beta, float* tA, float* tB, float* tC); //beware of inserting large angles! float wrap_pm_pi(float theta); From f4b934a78af8066f2e71cfae7c20058078d0fd57 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 8 Nov 2017 21:15:14 -0800 Subject: [PATCH 019/155] change info dump command to 'i' --- Firmware/MotorControl/commands.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/commands.c b/Firmware/MotorControl/commands.c index b3d5e4e0..6978a091 100644 --- a/Firmware/MotorControl/commands.c +++ b/Firmware/MotorControl/commands.c @@ -169,7 +169,7 @@ void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface if (numscan == 2 && motor_number < num_motors) { set_current_setpoint(&motors[motor_number], current_feed_forward); } - } else if(buffer[0] == 'e'){ // Generic ECHO command for testing + } else if(buffer[0] == 'i'){ // Dump device info // Retrieves the device signature, revision, flash size, and UUID printf("Signature: %#x\n", STM_ID_GetSignature()); printf("Revision: %#x\n", STM_ID_GetRevision()); @@ -381,4 +381,4 @@ void usb_update_thread() { } } vTaskDelete(osThreadGetId()); -} \ No newline at end of file +} From 1a06f54c48695e767c8dca29a156ff2f47c7fddc Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 9 Nov 2017 22:04:09 -0800 Subject: [PATCH 020/155] add explore_odrive.py --- tools/explore_odrive.py | 20 ++++++++++++++++++++ tools/test_communication.py | 7 ++----- 2 files changed, 22 insertions(+), 5 deletions(-) create mode 100644 tools/explore_odrive.py diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py new file mode 100644 index 00000000..84a0f10a --- /dev/null +++ b/tools/explore_odrive.py @@ -0,0 +1,20 @@ +""" +Load an odrive object to play with in the IPython interactive shell. +""" + +import odrive.core +my_odrive = odrive.core.find_any() + +print('') +print('ODRIVE EXPLORER') +print('') +print('Run this script with the following command:') +print('ipython -i explore_odrive.py') +print('') +print('You can now type "my_odrive." and press ') +print('This will present you with all the properties that you can reference') +print('') +print('For example: "my_odrive.motor0.encoder.pll_pos"') +print('will print the current encoder position on motor 0') +print('and "my_odrive.motor0.pos_setpoint = 10000"') +print('will send motor0 to 10000') diff --git a/tools/test_communication.py b/tools/test_communication.py index 1e37784c..3ee60c37 100755 --- a/tools/test_communication.py +++ b/tools/test_communication.py @@ -22,13 +22,8 @@ if __name__ == '__main__': import sys import time -import threading -import prompt_toolkit -import re -import tempfile import odrive.core - def noprint(str): pass @@ -55,6 +50,8 @@ def print_usage(): # that will leave that odrive variable in scope for the interactive session; just give the user some instructions # that they can then do stuff like odrive.[tabcomplete] +# EDIT: I just made the above: check explore_odrive.py + # We can also make a function odrive.send_legacy_cmd(cmd_str), which is important for some features that # we haven't ported yet. From 60f0873bc3c7a44c9252f34b80e823f9f860b200 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 12 Nov 2017 15:22:00 -0800 Subject: [PATCH 021/155] Firmware v0.2 release date --- Firmware/CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 7dea17c9..3d928d83 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,5 +1,5 @@ -## [0.2.0] - UNRELEASED +## [0.2.0] - 2017-11-12 ### Added * UART communication * Setting to select UART or Step/dir on GIPIO 1,2 From c6c0e36066bb451d14d90fe63b15e6e8a682620d Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 13 Nov 2017 12:29:09 +0100 Subject: [PATCH 022/155] fix osSemaphoteWait return type --- .../Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c | 5 +++-- .../Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c b/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c index 302fd2fa..32b7b87c 100644 --- a/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c +++ b/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c @@ -26,7 +26,7 @@ * *---------------------------------------------------------------------------- * - * Portions Copyright © 2016 STMicroelectronics International N.V. All rights reserved. + * Portions Copyright � 2016 STMicroelectronics International N.V. All rights reserved. * Portions Copyright (c) 2013 ARM LIMITED * All rights reserved. * Redistribution and use in source and binary forms, with or without @@ -819,7 +819,8 @@ osSemaphoreId osSemaphoreCreate (const osSemaphoreDef_t *semaphore_def, int32_t * @retval number of available tokens, or -1 in case of incorrect parameters. * @note MUST REMAIN UNCHANGED: \b osSemaphoreWait shall be consistent in every CMSIS-RTOS. */ -int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec) +// TODO: submit patch upstream +osStatus osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec) { TickType_t ticks; portBASE_TYPE taskWoken = pdFALSE; diff --git a/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h index a3585409..467cb745 100644 --- a/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h +++ b/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h @@ -26,7 +26,7 @@ * *---------------------------------------------------------------------------- * - * Portions Copyright © 2016 STMicroelectronics International N.V. All rights reserved. + * Portions Copyright � 2016 STMicroelectronics International N.V. All rights reserved. * Portions Copyright (c) 2013 ARM LIMITED * All rights reserved. * Redistribution and use in source and binary forms, with or without @@ -719,9 +719,9 @@ osSemaphoreId osSemaphoreCreate (const osSemaphoreDef_t *semaphore_def, int32_t /// Wait until a Semaphore token becomes available. /// \param[in] semaphore_id semaphore object referenced with \ref osSemaphoreCreate. /// \param[in] millisec timeout value or 0 in case of no time-out. -/// \return number of available tokens, or -1 in case of incorrect parameters. +/// \return osOK if the operation succeded. /// \note MUST REMAIN UNCHANGED: \b osSemaphoreWait shall be consistent in every CMSIS-RTOS. -int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec); +osStatus osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec); /// Release a Semaphore token. /// \param[in] semaphore_id semaphore object referenced with \ref osSemaphoreCreate. From d52686cd41813edcffc13597198eb69a0f7d8ff4 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 13 Nov 2017 17:26:31 +0100 Subject: [PATCH 023/155] miscellaneous protocol updates add back support for legacy protocol on UART add defines in commands.h for easy selection of protocols new protocol: use semaphores for sending and drop response after 10ms minor typing fixes --- Firmware/MotorControl/commands.cpp | 188 ++++++++++++------------ Firmware/MotorControl/commands.h | 17 +++ Firmware/MotorControl/legacy_commands.c | 52 +++++-- Firmware/MotorControl/legacy_commands.h | 9 +- Firmware/MotorControl/low_level.h | 6 +- Firmware/MotorControl/protocol.hpp | 6 +- Firmware/MotorControl/utils.c | 18 ++- Firmware/MotorControl/utils.h | 13 ++ 8 files changed, 198 insertions(+), 111 deletions(-) diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 4b3afa44..6beba64d 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -3,12 +3,14 @@ // TODO: remove this option // and once the legacy protocol is phased out, remove the seq-no hack in protocol.py +// todo: make clean switches for protocol #define ENABLE_LEGACY_PROTOCOL +#include "commands.h" #include "low_level.h" #include "protocol.hpp" #include "freertos_vars.h" -#include "commands.h" +#include "utils.h" #ifdef ENABLE_LEGACY_PROTOCOL #include "legacy_commands.h" @@ -42,6 +44,9 @@ static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx static uint8_t* usb_buf; static uint32_t usb_len; +// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable +static thread_local uint32_t deadline_ms = 0; + /* Variables exposed to USB & UART via read/write commands */ // TODO: include range information in JSON description @@ -121,8 +126,8 @@ const Endpoint endpoints[] = { Endpoint::make_property("pll_vel", &motors[0].encoder.pll_vel), Endpoint::make_property("pll_kp", &motors[0].encoder.pll_kp), Endpoint::make_property("pll_ki", &motors[0].encoder.pll_ki), - Endpoint::make_property("encoder_offset", reinterpret_cast(&motors[0].encoder.encoder_offset)), - Endpoint::make_property("encoder_state", reinterpret_cast(&motors[0].encoder.encoder_state)), + Endpoint::make_property("encoder_offset", &motors[0].encoder.encoder_offset), + Endpoint::make_property("encoder_state", &motors[0].encoder.encoder_state), Endpoint::close_tree(), Endpoint::make_function("set_pos_setpoint", &motors_0_set_pos_setpoint_func), Endpoint::make_property("pos_setpoint", &motors[0].set_pos_setpoint_args.pos_setpoint), @@ -196,33 +201,43 @@ const Endpoint endpoints[] = { constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]); +#if defined(USB_PROTOCOL_NEW) -// The USB channel is natively packet based but on some platforms (specifically -// macOS) it's not possible to directly access the device as a USB device. -// Instead, such platforms expose the device as a serial port, however that -// breaks our packet boundaries. For now we just neglect this. If you happen to -// be limited by such a platform, you should reconsider your life choices -// or as a workaround enable this: +class USBSender : public PacketSink { +public: + int process_packet(const uint8_t* buffer, size_t length) { + // cannot send partial packets + if (length > USB_TX_DATA_SIZE) + return -1; + // wait for USB interface to become ready + if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit packet + uint8_t status = CDC_Transmit_FS( + const_cast(buffer) /* casting this const away is safe because... + well... it's not actually. Stupid STM. */, length); + return (status == USBD_OK) ? 0 : -1; + } +} usb_sender; -//Oskar: Put switches like this at top of file -//#define STREAM_ON_USB +BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_sender); +#elif defined(USB_PROTOCOL_NEW_STREAM_BASED) - -#ifdef STREAM_ON_USB class USBSender : public StreamSink { public: int process_bytes(const uint8_t* buffer, size_t length) { // Loop to ensure all bytes get sent - // TODO: add timeout while (length) { size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - while (CDC_Transmit_FS( + // wait for USB interface to become ready + if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit chunk + if (CDC_Transmit_FS( const_cast(buffer) /* casting this const away is safe because... well... it's not actually. Stupid STM. */, chunk) != USBD_OK) - //Oskar: we made a semaphore sem_usb_tx that guards the USB tx resource, - // that you can wait for to see if busy. Check _write in syscalls.c on devel for example use - osDelay(1); + return -1; buffer += chunk; length -= chunk; } @@ -233,46 +248,29 @@ public: } usb_sender; PacketToStreamConverter usb_packet_sender(usb_sender); -BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_packet_sender); -StreamToPacketConverter usb_stream_sink(usb_connection); +BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender); +StreamToPacketConverter usb_stream_sink(usb_channel); -#else - -class USBSender : public PacketSink { -public: - int process_packet(const uint8_t* buffer, size_t length) { - // cannot send partial packets - if (length > USB_TX_DATA_SIZE) - return -1; - while (CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, length) != USBD_OK) - //Oskar: we made a semaphore sem_usb_tx that guards the USB tx resource, - // that you can wait for to see if busy. Check _write in syscalls.c on devel for example use - osDelay(1); - return 0; - } -} usb_sender; - -BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_sender); #endif +#if defined(UART_PROTOCOL_NEW) class UART4Sender : public StreamSink { public: int process_bytes(const uint8_t* buffer, size_t length) { - //Check length - if (length > UART_TX_BUFFER_SIZE) - return -1; - // Loop until the UART is ready - // TODO: implement ring buffer to get a more continuous stream of data - while (huart4.gState != HAL_UART_STATE_READY) - //Oskar: we made a semaphore sem_uart_dma that guards the UART tx resource, - // that you can wait for to see if busy. Check _write in syscalls.c on devel for example use - osDelay(1); - // memcpy data into uart_tx_buf - memcpy(tx_buf_, buffer, length); - // Start DMA background trasnfer - HAL_UART_Transmit_DMA(&huart4, tx_buf_, length); + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; + // wait for USB interface to become ready + // TODO: implement ring buffer to get a more continuous stream of data + if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit chunk + memcpy(tx_buf_, buffer, chunk); + if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) + return -1; + buffer += chunk; + length -= chunk; + } return 0; } @@ -282,8 +280,9 @@ private: } uart4_sender; PacketToStreamConverter uart4_packet_sender(uart4_sender); -BidirectionalPacketBasedChannel uart4_connection(endpoints, NUM_ENDPOINTS, uart4_packet_sender); -StreamToPacketConverter UART4_stream_sink(uart4_connection); +BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); +StreamToPacketConverter UART4_stream_sink(uart4_channel); +#endif /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ @@ -304,6 +303,8 @@ void init_communication(void) { void communication_task(void const * argument) { (void) argument; + +#if !defined(UART_PROTOCOL_NONE) //DMA open loop continous circular buffer //1ms delay periodic, chase DMA ptr around @@ -315,37 +316,65 @@ void communication_task(void const * argument) { // data out of the circular buffer into a parse buffer, controlled by a state machine HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; +#endif // Re-run state-machine forever for (;;) { +#if !defined(UART_PROTOCOL_NONE) // Check for UART errors and restart recieve DMA transfer if required if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { HAL_UART_AbortReceive(&huart4); HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); } // Fetch the circular buffer "write pointer", where it would write next - uint32_t rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; - // During sleeping, we may have fallen several characters behind, so we keep - // going until we are caught up, before we sleep again - while (rcv_idx != last_rcv_idx) { - // Fetch the next char, rotate read ptr - uint8_t c = dma_circ_buffer[last_rcv_idx]; - if (++last_rcv_idx == UART_RX_BUFFER_SIZE) - last_rcv_idx = 0; - //Oskar: we don't have to process 1 byte at a time, - // we can process up to MIN(last_rcv_idx, UART_RX_BUFFER_SIZE-1) - UART4_stream_sink.process_bytes(&c, 1); - } + uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); +#if defined(UART_PROTOCOL_NEW) + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < last_rcv_idx) { + UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, + UART_RX_BUFFER_SIZE - last_rcv_idx); + last_rcv_idx = 0; + } + if (new_rcv_idx > last_rcv_idx) { + UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx); + last_rcv_idx = new_rcv_idx; + } +#elif defined(UART_PROTOCOL_LEGACY) + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < last_rcv_idx) { + legacy_parse_stream(dma_circ_buffer + last_rcv_idx, + UART_RX_BUFFER_SIZE - last_rcv_idx); + last_rcv_idx = 0; + } + if (new_rcv_idx > last_rcv_idx) { + legacy_parse_stream(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx); + last_rcv_idx = new_rcv_idx; + } +#endif +#endif + +#if !defined(USB_PROTOCOL_NONE) // When we reach here, we are out of immediate characters to fetch out of UART buffer // Now we check if there is any USB processing to do: we wait for up to 1 ms, // before going back to checking UART again. const uint32_t usb_check_timeout = 1; // ms osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); if (sem_stat == osOK) { - USB_receive_packet(usb_buf, usb_len); + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); +#if defined(USB_PROTOCOL_NEW) + usb_channel.process_packet(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_NEW_STREAM_BASED) + usb_stream_sink.process_bytes(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_LEGACY) + legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); +#endif USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet } +#endif } // If we get here, then this task is done @@ -373,28 +402,3 @@ void usb_update_thread() { vTaskDelete(osThreadGetId()); } - -//Oskar: can you also do a ENABLE_LEGACY_PROTOCOL case for UART? -// If this has to be exclusive of the new protocol, that's fine: it -// lets us move on and upgrade the arduino library later. -// Please test that it still works on an arduino. - -void USB_receive_packet(const uint8_t *buffer, size_t length) { - //printf("[USB] got %d bytes, first is %c\r\n", length, buffer[0]); osDelay(5); -#ifdef ENABLE_LEGACY_PROTOCOL - const uint8_t* legacy_commands = (const uint8_t*)"pvcgsmo"; - while (*legacy_commands && length) { - if (buffer[0] == *(legacy_commands++)) { - //printf("[USB] process legacy command %c\r\n", buffer[0]); osDelay(5); - legacy_parse_cmd(buffer, length); - length = 0; - } - } -#endif - -#ifdef STREAM_ON_USB - usb_stream_sink.process_bytes(buffer, length); -#else - usb_connection.process_packet(buffer, length); -#endif -} diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 5714ba0e..0469c10a 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -1,3 +1,5 @@ +#ifndef COMMANDS_H +#define COMMANDS_H // TODO: resolve assert #define assert(expr) @@ -8,6 +10,19 @@ #include #include "crc.hpp" +// Select which protocol to run on UART (see README for more details) +//#define UART_PROTOCOL_NEW +#define UART_PROTOCOL_LEGACY +//#define UART_PROTOCOL_NONE + +// Select which protocol to run on USB (see README for more details) +#define USB_PROTOCOL_NEW +//#define USB_PROTOCOL_NEW_STREAM_BASED +//#define USB_PROTOCOL_LEGACY +//#define USB_PROTOCOL_NONE + + + typedef enum { GPIO_MODE_UART, GPIO_MODE_STEP_DIR, @@ -24,4 +39,6 @@ void USB_receive_packet(const uint8_t *buffer, size_t length); #ifdef __cplusplus } +#endif + #endif /* COMMANDS_H */ diff --git a/Firmware/MotorControl/legacy_commands.c b/Firmware/MotorControl/legacy_commands.c index c52a2a5d..254f9dd7 100644 --- a/Firmware/MotorControl/legacy_commands.c +++ b/Firmware/MotorControl/legacy_commands.c @@ -78,13 +78,13 @@ float* exposed_floats[] = { int* exposed_ints[] = { (int*)&motors[0].control_mode, // rw - &motors[0].encoder.encoder_offset, // rw - &motors[0].encoder.encoder_state, // ro - &motors[0].error, // rw + (int*)&motors[0].encoder.encoder_offset, // rw + (int*)&motors[0].encoder.encoder_state, // ro + (int*)&motors[0].error, // rw (int*)&motors[1].control_mode, // rw - &motors[1].encoder.encoder_offset, // rw - &motors[1].encoder.encoder_state, // ro - &motors[1].error, // rw + (int*)&motors[1].encoder.encoder_offset, // rw + (int*)&motors[1].encoder.encoder_state, // ro + (int*)&motors[1].error, // rw }; bool* exposed_bools[] = { @@ -113,13 +113,13 @@ static void print_monitoring(int limit); /* Function implementations --------------------------------------------------*/ -void legacy_parse_cmd(const uint8_t* buffer, int len) { +void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_capacity, SerialPrintf_t response_interface) { // Set response interface - serial_printf_select = SERIAL_PRINTF_IS_USB; + serial_printf_select = response_interface; // Cast away const and write beyond the array bounds. Because we can. // (TODO: yeah maybe not, but this should be gone once we disable legacy commands) - ((uint8_t *)buffer)[len <= 63 ? len : 63] = 0; + ((uint8_t *)buffer)[len < buffer_capacity ? len : (buffer_capacity - 1)] = 0; // check incoming packet type if (buffer[0] == 'p') { @@ -236,6 +236,40 @@ void legacy_parse_cmd(const uint8_t* buffer, int len) { serial_printf_select = SERIAL_PRINTF_IS_UART; } +void legacy_parse_stream(const uint8_t* buffer, size_t len) { + #define PARSE_BUFFER_SIZE 64 + static uint8_t parse_buffer[PARSE_BUFFER_SIZE]; + static bool read_active = false; + static uint32_t parse_buffer_idx = 0; + + while (len--) { + // Fetch the next char + uint8_t c = *(buffer++); + // Look for start character + if (c == '$') { + read_active = true; + continue; // do not record start char + } + // Record into parse buffer when actively reading + if (read_active) { + parse_buffer[parse_buffer_idx++] = c; + if (c == '\r' || c == '\n' || c == '!') { + // End of command string + legacy_parse_cmd(parse_buffer, parse_buffer_idx, PARSE_BUFFER_SIZE, SERIAL_PRINTF_IS_UART); + // Reset receieve state machine + read_active = false; + parse_buffer_idx = 0; + } else if (parse_buffer_idx == PARSE_BUFFER_SIZE - 1) { + // We are not at end of command, and receiving another character after this + // would go into the last slot, which is reserved for terminating null. + // We have effectively overflowed parse buffer: abort. + read_active = false; + parse_buffer_idx = 0; + } + } + } +} + static void print_monitoring(int limit) { serial_printf_select = SERIAL_PRINTF_IS_USB; diff --git a/Firmware/MotorControl/legacy_commands.h b/Firmware/MotorControl/legacy_commands.h index 6ba81239..bb945dab 100644 --- a/Firmware/MotorControl/legacy_commands.h +++ b/Firmware/MotorControl/legacy_commands.h @@ -1,5 +1,5 @@ -#ifndef COMMANDS_H -#define COMMANDS_H +#ifndef LEGACY_COMMANDS_H +#define LEGACY_COMMANDS_H #ifdef __cplusplus extern "C" { @@ -27,10 +27,11 @@ extern uint16_t* exposed_uint16[]; /* Exported functions --------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ -void legacy_parse_cmd(const uint8_t* buffer, int len); +void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_length, SerialPrintf_t response_interface); +void legacy_parse_stream(const uint8_t* buffer, size_t len); #ifdef __cplusplus } #endif -#endif /* COMMANDS_H */ +#endif /* LEGACY_COMMANDS_H */ diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index e3cfd829..13f7161a 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -102,8 +102,8 @@ typedef struct { typedef struct { TIM_HandleTypeDef* encoder_timer; int encoder_cpr; - int encoder_offset; - int encoder_state; + int32_t encoder_offset; + int32_t encoder_state; int motor_dir; // 1/-1 for fwd/rev alignment to encoder. float phase; float pll_pos; @@ -122,7 +122,7 @@ typedef struct { Motor_control_mode_t control_mode; bool enable_step_dir; float counts_per_step; - int error; + Error_t error; float pos_setpoint; float pos_gain; float vel_setpoint; diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index 3ff2058c..5069aed0 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -68,6 +68,8 @@ constexpr uint16_t PROTOCOL_VERSION = 1; constexpr uint16_t TX_BUF_SIZE = 32; // does not work with 64 for some reason constexpr uint16_t RX_BUF_SIZE = 128; // larger values than 128 have currently no effect because of protocol limitations +// Maximum time we allocate for processing and responding to a request +constexpr uint32_t PROTOCOL_SERVER_TIMEOUT_MS = 10; template inline size_t write_le(T value, uint8_t* buffer); @@ -166,8 +168,8 @@ static inline T read_le(const uint8_t** buffer, size_t* length) { class PacketSink { public: // @brief Processes a packet. + // The blocking behavior shall depend on the thread-local deadline_ms variable. // @return: 0 on success, otherwise a non-zero error code - // TODO: add deadline parameter. Currently all implementations block until they can send everything. // TODO: define what happens when the packet is larger than what the implementation can handle. virtual int process_packet(const uint8_t* buffer, size_t length) = 0; }; @@ -175,8 +177,8 @@ public: class StreamSink { public: // @brief Processes a chunk of bytes that is part of a continuous stream. + // The blocking behavior shall depend on the thread-local deadline_ms variable. // @return: 0 on success, otherwise a non-zero error code - // TODO: add deadline parameter. Currently all implementations block until they can send everything. virtual int process_bytes(const uint8_t* buffer, size_t length) = 0; // @brief Returns the number of bytes that can still be written to the stream. diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index e28d70a9..b6fbd0d5 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -1,6 +1,7 @@ #include #include +#include static const float one_by_sqrt3 = 0.57735026919f; static const float two_by_sqrt3 = 1.15470053838f; @@ -166,4 +167,19 @@ float fast_atan2(float y, float x) { int mod(int dividend, int divisor){ int r = dividend % divisor; return (r < 0) ? (r + divisor) : r; -} \ No newline at end of file +} + +// @brief: Returns how much time is left until the deadline is reached. +// If the deadline has already passed, the return value is 0 (except if +// the deadline is very far in the past) +uint32_t deadline_to_timeout(uint32_t deadline_ms) { + uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency); + uint32_t timeout_ms = deadline_ms - now_ms; + return (timeout_ms & 0x80000000) ? 0 : timeout_ms; +} + +// @brief: Converts a timeout to a deadline based on the current time. +uint32_t timeout_to_deadline(uint32_t timeout_ms) { + uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency); + return now_ms + timeout_ms; +} diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 542b7dd7..92436b74 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -2,6 +2,12 @@ #ifndef __UTILS_H #define __UTILS_H +#ifdef __cplusplus +extern "C" { +#endif + +#include + /** * @brief Unique ID register address location */ @@ -82,4 +88,11 @@ float wrap_pm_pi(float theta); float fast_atan2(float y, float x); int mod(int dividend, int divisor); +uint32_t deadline_to_timeout(uint32_t deadline_ms); +uint32_t timeout_to_deadline(uint32_t timeout_ms); + +#ifdef __cplusplus +} +#endif + #endif //__UTILS_H From 91f61c076b6a55f72a4387ddc5e031c6c7d1ea92 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 13 Nov 2017 19:37:17 +0100 Subject: [PATCH 024/155] miscelleneous python updates accept arguments in explore_odrive.py (like in test_communication.py) implement --discover argument fix spurious property fetch fix read-only properties better serial port detection for windows revert demo.py --- tools/demo.py | 11 +++---- tools/explore_odrive.py | 66 ++++++++++++++++++++++++++++++++++--- tools/odrive/core.py | 61 +++++++++++++++------------------- tools/odrive/protocol.py | 37 ++++++++++++--------- tools/test_communication.py | 31 ++++++++--------- 5 files changed, 127 insertions(+), 79 deletions(-) mode change 100644 => 100755 tools/explore_odrive.py diff --git a/tools/demo.py b/tools/demo.py index fa148eba..ab2fbe2a 100755 --- a/tools/demo.py +++ b/tools/demo.py @@ -8,10 +8,7 @@ import time import math # Find a connected ODrive (this will block until you connect one) -odrives = odrive.core.find_all(printer=print) -odrives = list(odrives) #force eval of generator to test finding functions -my_drive = odrives[0] -# my_drive = odrive.core.find_any(printer=print) +my_drive = odrive.core.find_any(consider_usb=True, consider_serial=False, printer=print) # The above call returns a python object with a dynamically generated type. The # type hierarchy will correspond to the endpoint list in `MotorControl/protocol.cpp`. @@ -30,9 +27,9 @@ print("Position setpoint is " + str(my_drive.motor0.pos_setpoint)) # And this is how function calls are done: my_drive.motor0.set_pos_setpoint(0.0, 0.0, 0.0) -# little sine wave to test +# A little sine wave to test t0 = time.monotonic() -while False: +while True: setpoint = 10000.0 * math.sin((time.monotonic() - t0)*2) print("goto " + str(int(setpoint))) my_drive.motor0.set_pos_setpoint(setpoint, 0.0, 0.0) @@ -45,4 +42,4 @@ while False: my_drive.vbus_voltage = 11.0 # fails with `AttributeError: can't set attribute` # Assign an incompatible value: -# my_drive.motor0.pos_setpoint = "I like trains" # fails with `TypeError: expected value of type float` +my_drive.motor0.pos_setpoint = "I like trains" # fails with `ValueError: could not convert string to float` diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py old mode 100644 new mode 100755 index 84a0f10a..7c2a22c2 --- a/tools/explore_odrive.py +++ b/tools/explore_odrive.py @@ -1,16 +1,66 @@ +#!/usr/bin/env python3 """ Load an odrive object to play with in the IPython interactive shell. """ import odrive.core -my_odrive = odrive.core.find_any() +import argparse +import sys + + +# Parse arguments +parser = argparse.ArgumentParser(description='Load an odrive object to play with in the IPython interactive shell.') +parser.add_argument("-v", "--verbose", action="store_true", + help="print debug information") +group = parser.add_mutually_exclusive_group() +group.add_argument("-d", "--discover", metavar="CHANNELS", action="store", + help="Automatically discover ODrives. Takes a comma-separated list (without spaces) " + "to indicate which connection types should be considered. Possible values are " + "usb and serial. For example \"--discover=usb,serial\" indicates " + "that USB and serial ports should be scanned for ODrives. " + "If none of the below options are specified, --discover=usb is assumed.") +group.add_argument("-u", "--usb", metavar="BUS:DEVICE", action="store", + help="Specifies the USB port on which the device is connected. " + "For example \"001:014\" means bus 001, device 014. The numbers can be obtained " + "using `lsusb`.") +group.add_argument("-s", "--serial", metavar="PORT", action="store", + help="Specifies the serial port on which the device is connected. " + "For example \"/dev/ttyUSB0\". Use `ls /dev/tty*` to find your port name.") +parser.set_defaults(discover="usb") +args = parser.parse_args() + +if (args.verbose): + printer = print +else: + printer = lambda x: None + + +# Connect to device +if not args.usb is None: + try: + bus = int(args.usb.split(":")[0]) + address = int(args.usb.split(":")[1]) + except (ValueError, IndexError): + print("the --usb argument must look something like this: \"001:014\"") + sys.exit(1) + try: + my_odrive = odrive.core.open_usb(bus, address, printer=printer) + except odrive.protocol.DeviceInitException as ex: + print(str(ex)) + sys.exit(1) +elif not args.serial is None: + my_odrive = odrive.core.open_serial(args.serial, printer=printer) +else: + print("Waiting for device...") + consider_usb = 'usb' in args.discover.split(',') + consider_serial = 'serial' in args.discover.split(',') + my_odrive = odrive.core.find_any(consider_usb, consider_serial, printer=printer) +print("Connected!") + print('') print('ODRIVE EXPLORER') print('') -print('Run this script with the following command:') -print('ipython -i explore_odrive.py') -print('') print('You can now type "my_odrive." and press ') print('This will present you with all the properties that you can reference') print('') @@ -18,3 +68,11 @@ print('For example: "my_odrive.motor0.encoder.pll_pos"') print('will print the current encoder position on motor 0') print('and "my_odrive.motor0.pos_setpoint = 10000"') print('will send motor0 to 10000') +print('') + +# Enter interactive python shell with tab complete enabled +import code +import rlcompleter +import readline +readline.parse_and_bind("tab: complete") +code.interact(local=locals(), banner='') diff --git a/tools/odrive/core.py b/tools/odrive/core.py index bd528b28..c4888de7 100644 --- a/tools/odrive/core.py +++ b/tools/odrive/core.py @@ -7,11 +7,12 @@ import time import json import usb.core import usb.util +import serial +import serial.tools.list_ports import odrive.util import odrive.usbbulk_transport import odrive.serial_transport import re -import serial import time import os import odrive.protocol @@ -44,15 +45,7 @@ class SimpleDeviceProperty(property): return struct.unpack(self._struct_format, buffer)[0] def fset(self, obj, value): - #Oskar: Pythonic duck typing style means that you should pretend that types are - # compatible, and catch errors. So instead do something like: - # value = self._type(value) - # you could of course wrap this in a try/except block, but when it fails it - # raises a TypeError, just like the one you made below, so I'd just let that fire - # by itself. - - if not isinstance(value, self._type): - raise TypeError("expected value of type {}".format(self._type.__name__)) + value = self._type(value) buffer = struct.pack(self._struct_format, value) # TODO: Currenly we wait for an ack here. Settle on the default guarantee. self._channel.remote_endpoint_operation(self._id, buffer, True, 0) @@ -68,17 +61,15 @@ def call_remote_function(channel, trigger_id, arg_properties, *args): arg_properties[i].fset(None, args[i]) channel.remote_endpoint_operation(trigger_id, None, True, 0) -#Oskar: setattr_or_raise_if_undefined -def raise_if_undefined(self, name, value): +def setattr_or_raise_if_undefined(self, name, value): """ If employed as an object's __setattr__ function, this function makes sure that an assignment to an undefined attribute doesn't create a new attribute but instead raises an exception """ - #Oskar: hasattr internally calls fget to determine if the attribute exists, - # which unnessecarily creates bus traffic. We should try to solve this. - # Step-in on the hasattr line in the debugger to see this. - if hasattr(self, name): + # We can't use hasattr here because internally it fetches the property + # value, creating unnecessary bus traffic + if name in dir(self): object.__setattr__(self, name, value) else: raise TypeError('Cannot set name %r on object of type %s' % ( @@ -128,9 +119,7 @@ def create_property(name, json_data, channel, printer): printer("property {} has no specified ID".format(name)) return None - #Oskar: Bug: json_data calls this "access", but we look for "mode". - # The default should probably be "r" anyway, it's safer I'd say. - access_mode = json_data.get("mode", "rw") + access_mode = json_data.get("access", "r") return SimpleDeviceProperty(channel, id_str, property_type, struct_format, 'r' in access_mode, @@ -162,7 +151,7 @@ def create_object(name, json_data, namespace, channel, printer=noprint): namespace = name # Build attribute list from JSON - attributes = {"__setattr__": raise_if_undefined} + attributes = {"__setattr__": setattr_or_raise_if_undefined} for member in json_data.get("members", []): member_name = member.get("name", None) if member_name is None: @@ -208,11 +197,11 @@ def channel_from_serial_port(port, baud, packet_based, printer=noprint): if packet_based == True: # TODO: implement packet based transport over serial raise NotImplementedError("not supported yet") - serial_device = odrive.serial_transport.SerialStreamTransport(port, 115200) + serial_device = odrive.serial_transport.SerialStreamTransport(port, baud) input_stream = odrive.protocol.PacketFromStreamConverter(serial_device) output_stream = odrive.protocol.PacketToStreamConverter(serial_device) return odrive.protocol.Channel( - "serial port {}@{}".format(port, 115200), + "serial port {}@{}".format(port, baud), input_stream, output_stream) def object_from_channel(channel, printer=noprint): @@ -262,6 +251,9 @@ def find_dev_serial_ports(search_regex): except FileNotFoundError: return [] +def find_pyserial_ports(): + return [x.name for x in serial.tools.list_ports.comports()] + def find_serial_channels(printer=noprint): """ Scans for serial ports. @@ -269,19 +261,15 @@ def find_serial_channels(printer=noprint): Not every returned object necessarily represents a compatible device. """ - #Oskar: Why not just use this tool to find the available ports? - # https://pyserial.readthedocs.io/en/latest/tools.html#module-serial.tools.list_ports - - # Real serial ports or USB-Serial converters - linux_real_serial_ports = find_dev_serial_ports(r'^ttyUSB') - windows_real_serial_ports = [ "COM1", "COM2", "COM3", "COM4" ] + # Real serial ports or USB-Serial converters (tested on Linux and Windows) + real_serial_ports = find_pyserial_ports() # Serial devices that are exposed by the platform # for the device's USB connection linux_usb_serial_ports = find_dev_serial_ports(r'^ttyACM') macos_usb_serial_ports = find_dev_serial_ports(r'^tty\.usbmodem') - for port in linux_real_serial_ports + windows_real_serial_ports + linux_usb_serial_ports + macos_usb_serial_ports: + for port in real_serial_ports + linux_usb_serial_ports + macos_usb_serial_ports: try: yield channel_from_serial_port(port, 115200, False, printer) except serial.serialutil.SerialException: @@ -289,13 +277,16 @@ def find_serial_channels(printer=noprint): continue -def find_all(printer=noprint): +def find_all(consider_usb=True, consider_serial=False, printer=noprint): """ Returns a generator with all the connected devices that speak the ODrive protocol """ - usb_channels = find_usb_channels(printer=printer) - serial_channels = find_serial_channels(printer=printer) - for channel in itertools.chain(usb_channels, serial_channels): + channels = iter(()) + if (consider_usb): + channels = itertools.chain(channels, find_usb_channels(printer=printer)) + if (consider_serial): + channels = itertools.chain(channels, find_serial_channels(printer=printer)) + for channel in channels: # TODO: blacklist known bad channels try: yield object_from_channel(channel, printer) @@ -304,7 +295,7 @@ def find_all(printer=noprint): continue -def find_any(printer=noprint): +def find_any(consider_usb=True, consider_serial=False, printer=noprint): """ Scans for ODrives on all supported interfaces and returns the first device that is found. If no device is connected the function blocks. @@ -314,7 +305,7 @@ def find_any(printer=noprint): # poll for device printer("looking for ODrive...") while True: - dev = next(find_all(printer=printer), None) + dev = next(find_all(consider_usb, consider_serial, printer=printer), None) if dev is not None: return dev printer("no device found") diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index e66dc1f5..bef172d5 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -2,6 +2,7 @@ import time import struct +from abc import ABC, abstractmethod SYNC_BYTE = 0xAA CRC8_INIT = 0x42 @@ -56,21 +57,26 @@ class ChannelBrokenException(Exception): class DeviceInitException(Exception): pass -#Oskar: I would just get rid of these "abstract classes", -# I think just looking and seeing that the classes have -# a process_packet or get_packet is enough. -class StreamSource(object): - pass +class StreamSource(ABC): + @abstractmethod + def get_bytes(self, deadline): + pass -class StreamSink(object): - pass +class StreamSink(ABC): + @abstractmethod + def process_bytes(self, bytes): + pass -class PacketSource(object): - pass +class PacketSource(ABC): + @abstractmethod + def get_packet(self, deadline): + pass -class PacketSink(object): - pass +class PacketSink(ABC): + @abstractmethod + def process_packet(self, packet): + pass class StreamToPacketConverter(StreamSink): @@ -174,9 +180,8 @@ class Channel(PacketSink): _interface_definition_crc = 0 _expected_acks = {} - # Chose these parameters to be sensible for a specific transport layer - #Oskar: it's a timeout, not delay. - _resend_delay = 5.0 # [s] + # Choose these parameters to be sensible for a specific transport layer + _resend_timeout = 5.0 # [s] _send_attempts = 5 def __init__(self, name, input, output): @@ -219,7 +224,7 @@ class Channel(PacketSink): attempt = 0 while (attempt < self._send_attempts): self._output.process_packet(packet) - deadline = time.monotonic() + self._resend_delay + deadline = time.monotonic() + self._resend_timeout # Read and process packets until we get an ack or need to resend # TODO: support I/O driven reception (wait on semaphore) while True: @@ -248,7 +253,7 @@ class Channel(PacketSink): buffer = bytes() while True: chunk_length = 64 - chunk = self.remote_endpoint_operation(0, struct.pack(" Date: Mon, 13 Nov 2017 22:41:14 +0100 Subject: [PATCH 025/155] update communication documentation --- Firmware/MotorControl/commands.cpp | 18 ++--- Firmware/MotorControl/commands.h | 8 ++- Firmware/MotorControl/protocol.hpp | 46 +----------- Firmware/README.md | 109 ++++++++++------------------- Firmware/legacy-protocol.md | 68 ++++++++++++++++++ Firmware/protocol.md | 69 ++++++++++++++++++ tools/explore_odrive.py | 4 +- tools/test_communication.py | 4 +- 8 files changed, 194 insertions(+), 132 deletions(-) create mode 100644 Firmware/legacy-protocol.md create mode 100644 Firmware/protocol.md diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 6beba64d..2ae8939f 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -35,9 +35,11 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; extern USBD_HandleTypeDef hUsbDeviceFS; /* Private constant data -----------------------------------------------------*/ -// TODO: make command to switch gpio_mode during run-time +#if defined(USE_GPIO_MODE_STEP_DIR) +static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir +#else static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx -// static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir +#endif /* Private variables ---------------------------------------------------------*/ @@ -201,7 +203,7 @@ const Endpoint endpoints[] = { constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]); -#if defined(USB_PROTOCOL_NEW) +#if defined(USB_PROTOCOL_NATIVE) class USBSender : public PacketSink { public: @@ -222,7 +224,7 @@ public: BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_sender); -#elif defined(USB_PROTOCOL_NEW_STREAM_BASED) +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) class USBSender : public StreamSink { public: @@ -253,7 +255,7 @@ StreamToPacketConverter usb_stream_sink(usb_channel); #endif -#if defined(UART_PROTOCOL_NEW) +#if defined(UART_PROTOCOL_NATIVE) class UART4Sender : public StreamSink { public: int process_bytes(const uint8_t* buffer, size_t length) { @@ -330,7 +332,7 @@ void communication_task(void const * argument) { uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); -#if defined(UART_PROTOCOL_NEW) +#if defined(UART_PROTOCOL_NATIVE) // Process bytes in one or two chunks (two in case there was a wrap) if (new_rcv_idx < last_rcv_idx) { UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, @@ -365,9 +367,9 @@ void communication_task(void const * argument) { osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); if (sem_stat == osOK) { deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); -#if defined(USB_PROTOCOL_NEW) +#if defined(USB_PROTOCOL_NATIVE) usb_channel.process_packet(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_NEW_STREAM_BASED) +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) usb_stream_sink.process_bytes(usb_buf, usb_len); #elif defined(USB_PROTOCOL_LEGACY) legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 0469c10a..6d44685c 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -11,16 +11,18 @@ #include "crc.hpp" // Select which protocol to run on UART (see README for more details) -//#define UART_PROTOCOL_NEW +//#define UART_PROTOCOL_NATIVE #define UART_PROTOCOL_LEGACY //#define UART_PROTOCOL_NONE // Select which protocol to run on USB (see README for more details) -#define USB_PROTOCOL_NEW -//#define USB_PROTOCOL_NEW_STREAM_BASED +#define USB_PROTOCOL_NATIVE +//#define USB_PROTOCOL_NATIVE_STREAM_BASED //#define USB_PROTOCOL_LEGACY //#define USB_PROTOCOL_NONE +// Use GPIO 1/2 for step/dir input instead of UART +#define USE_GPIO_MODE_STEP_DIR typedef enum { diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index 5069aed0..92664d55 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -1,49 +1,5 @@ /* -* # ODrive Communication Protocol # -* -* Communicating with an ODrive consists of a series of endpoint operations. -* An endpoint can be any data representation that can be serialized. -* There is a default seralization implementation for POD types; for custom types -* you must (de)seralize yourself. In the future we may provide a default seralizer -* for stucts. -* The available endpoints can be enumerated by reading the JSON from endpoint 0 -* and can theoretically be different for each communication interface (they are not in practice). -* -* Each endpoint operation can send bytes to one endpoint (referenced by it's ID) -* and at the same time receive bytes from the same endpoint. The semantics of -* these payloads are specific to each endpoint's type, the name of which is -* indicated in the JSON. -* -* For instance an int32 endpoint's input and output is a 4 byte little endian -* representation. In general the convention for combined read/write requests is -* _exchange_, i.e. the returned value is the old value. Custom endpoint handlers -* may be non-compliant. -* -* ## Stream format: ## -* (For instance UART) -* -* 1. sync byte -* 2. packet length (0-127, larger values are reserved) -* 3. crc8(sync byte + packet length) -* 4. packet (as per below) -* 5. crc16(packet) -* -* ## Packet format: ## -* (For instance USB) -* -* __Request__ -* -* 1. seq-no, MSB = 0 -* 2. endpoint-id, MSB = "expect ack" -* 3. expected_response_size -* 4. payload (contains offset if required) -* 5. crc16(protocol_version + JSON) or just protocol_version for endpoint 0 -* -* __Response__ -* -* 1. seq-no, MSB = 1 -* 2. payload -* +see protocol.md for the protocol specification */ #ifndef __PROTOCOL_HPP diff --git a/Firmware/README.md b/Firmware/README.md index ec9bf71e..ed6efe52 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -23,10 +23,28 @@ The first thing to set is your board hardware version, located at the top of [In ``` ### Communication configuration -If you are using USB only to communicate with the ODrive, you may skip this step. +If want to use the example python scripts and connect the ODrive via USB, the defaults are fine for you and you can skip this step. -The GPIO 1,2 pins are configurable as either step/direction, or as UART. -In [MotorControl/commands.c](MotorControl/commands.c) please set `gpio_mode` to the corresponding value (`GPIO_MODE_UART` or `GPIO_MODE_STEP_DIR`). +You can select what interface you want to run on USB and UART. The following options are available: + +__USB__: + - `USB_PROTOCOL_NATIVE`: Use the native protocol (recommended for new applications). + The python library only understands the native protocol, so this is the way to go + if you use that. + - `USB_PROTOCOL_NATIVE_STREAM_BASED`: Use the native stream based protocol. + On most platforms the device shows up as a serial port when connected over USB. + So instead of using the python tool's direct USB access, you can use this option and then pretend you connected the device over serial. + __On some platforms (specifically macOS), this is required__ because the kernel doesn't allow direct USB access. + - `USB_PROTOCOL_LEGACY`: Use the human-readable legacy protocol + Select this option if you already have an existing application. This option will be removed in the future. + - `USB_PROTOCOL_NONE`: Ignore USB communication + +__GPIO 1,2 pins__: + - `UART_PROTOCOL_NATIVE`: Use the native protocol (see notes above). + - `UART_PROTOCOL_LEGACY`: Use the human-readable legacy protocol + Use this option if you control the ODrive with an Arduino. The ODrive Arduino library is not yet updated to the native protocol. + - `UART_PROTOCOL_NONE`: Ignore UART communication + - `USE_GPIO_MODE_STEP_DIR`: Step/direction control mode (use in conjunction with `UART_PROTOCOL_NONE`) ### Motor control parameters The rest of all the parameters are at the top of the [MotorControl/low_level.c](MotorControl/low_level.c) file. Please note that many parameters occur twice, once for each motor. @@ -108,90 +126,37 @@ After installing all of the above, open a Git Bash shell. Continue at section [B Run `make gdb`. This will reset and halt at program start. Now you can set breakpoints and run the program. If you know how to use gdb, you are good to go. If you prefer to debug from eclipse, see [Setting up Eclipse development environment](#setting-up-eclipse-development-environment). -## Communicating over USB -There is currently a very primitive method to read/write configuration, commands and errors from the ODrive over the USB. -Please use the `tools/test_communication.py` python script for this. It is written for [Python 3](https://www.python.org/downloads/) and so should be installed first. +## Communicating over USB or UART -* Assuming you already have Python, install dependencies: +### From Linux/Windows/macOS +There are two simple python scripts to help you get started with controlling the ODrive using python. + +1. [Install Python 3](https://www.python.org/downloads/), then install dependencies: ``` pip install pyusb pyserial prompt_toolkit ``` -* __Linux__: set up USB permissions +3. __Linux__: set up USB permissions ``` echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d[0-9][0-9]", MODE="0666"' | sudo tee /etc/udev/rules.d/50-odrive.rules sudo udevadm control --reload-rules sudo udevadm trigger # until you reboot you may need to do this everytime you reset the ODrive ``` -* Power the ODrive board (as per the [Flashing the firmware](#flashing-the-firmware) step) -* Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC -* __Windows__: Use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb. +4. Power the ODrive board (as per the [Flashing the firmware](#flashing-the-firmware) step) +5. Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC +6. __Windows__: Use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb. * If 'Odrive V3.x' is not in the list of devices upon opening Zadig check 'List All Devices' from the options menu. Connecting to the Odrive board directly and not over a usb hub may also help. With the Odrive selected in the device list choose 'libusb-win32' from the target driver list and select the large 'install driver' button. -* Run `tools/test_communication.py` +7. Run `./tools/demo.py` or `./tools/explore_odrive.py`. + - `demo.py` is a very simple script which will make motor 0 turn back and forth. Take a look at the code if you want to control the ODrive yourself programatically. + - `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --discover usb`. -### Command set -The most accurate way to understand the commands is to read [the code](MotorControl/commands.c) that parses the commands. Also you can have a look at the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) that makes it easy to use the UART interface on Arduino. You can also look at it as an implementation example of how to talk to the ODrive over UART. +### From Arduino -#### UART framing -USB communicates with packets, so it is easy to frame a command as one command per packet. However, UART doesn't have any packeting, so we need a way to frame the commands. The start-of-packet symbol is `$` and the end-of-packet symbol is `!`, that is, something like this: `$command!`. An example of a valid UART position command: -``` -$p 0 10000 0 0! -``` +[See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) -#### Motor Position command -``` -p motor position velocity_ff current_ff -``` -* `p` for position -* `motor` is the motor number, `0` or `1`. -* `position` is the desired position, in encoder counts. -* `velocity_ff` is the velocity feed-forward term, in counts/s. -* `current_ff` is the current feed-forward term, in A. +### Other platforms -Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. +See the [protocol specification](https://github.com/madcowswe/ODrive/blob/devel/Firmware/protocol.md) or the [legacy protocol specification](https://github.com/madcowswe/ODrive/blob/devel/Firmware/legacy-protocol.md). -#### Motor Velocity command -``` -v motor velocity current_ff -``` -* `v` for velocity -* `motor` is the motor number, `0` or `1`. -* `velocity` is the desired velocity in counts/s. -* `current_ff` is the current feed-forward term, in A. - -Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. - -#### Motor Current command -``` -c motor current -``` -* `c` for current -* `motor` is the motor number, `0` or `1`. -* `current` is the desired current in A. - -#### Variable getting and setting -``` -g type index -s type index value -``` -* `g` for get, `s` for set -* `type` is the data type as follows: -** `0` is float -** `1` is int -** `2` is bool -* `index` is the index in the corresponding [exposed variable table](MotorControl/commands.c). - -For example -* `g 0 12` will return the phase resistance of M0 -* `s 0 8 10000.0` will set the velocity limit on M0 to 10000 counts/s -* `g 1 3` will return the error status of M0 -* `g 1 7` will return the error status of M1 - -The error status corresponds to the [Error_t enum in low_level.h](MotorControl/low_level.h). - -Note that the links in this section are to a specific commits to make sure that the line numbers are accurate. That is, they don't link to the newest master, but to an old version. Please check the corresponding lines in the code you are using. This is especially important to get the correct indicies in the exposed variable tables, and the error enum values. - -#### Continous monitoring of variables -You can set up variables in monitoring slots, and then have them (or a subset of them) repeatedly printed upon request. Please see the code for this. ## Generating startup code **Note:** You do not need to run this step to program the board. This is only required if you wish to update the auto generated code. diff --git a/Firmware/legacy-protocol.md b/Firmware/legacy-protocol.md new file mode 100644 index 00000000..6f915bed --- /dev/null +++ b/Firmware/legacy-protocol.md @@ -0,0 +1,68 @@ + +Warning: [this protocol has been replaced](https://github.com/madcowswe/ODrive/blob/devel/Firmware/protocol.md). +It's still operational but for new applications it's recommended to use the new protocol. + +### Command set +The most accurate way to understand the commands is to read [the code](MotorControl/commands.c) that parses the commands. Also you can have a look at the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) that makes it easy to use the UART interface on Arduino. You can also look at it as an implementation example of how to talk to the ODrive over UART. + +#### UART framing +USB communicates with packets, so it is easy to frame a command as one command per packet. However, UART doesn't have any packeting, so we need a way to frame the commands. The start-of-packet symbol is `$` and the end-of-packet symbol is `!`, that is, something like this: `$command!`. An example of a valid UART position command: +``` +$p 0 10000 0 0! +``` + +#### Motor Position command +``` +p motor position velocity_ff current_ff +``` +* `p` for position +* `motor` is the motor number, `0` or `1`. +* `position` is the desired position, in encoder counts. +* `velocity_ff` is the velocity feed-forward term, in counts/s. +* `current_ff` is the current feed-forward term, in A. + +Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. + +#### Motor Velocity command +``` +v motor velocity current_ff +``` +* `v` for velocity +* `motor` is the motor number, `0` or `1`. +* `velocity` is the desired velocity in counts/s. +* `current_ff` is the current feed-forward term, in A. + +Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. + +#### Motor Current command +``` +c motor current +``` +* `c` for current +* `motor` is the motor number, `0` or `1`. +* `current` is the desired current in A. + +#### Variable getting and setting +``` +g type index +s type index value +``` +* `g` for get, `s` for set +* `type` is the data type as follows: +** `0` is float +** `1` is int +** `2` is bool +* `index` is the index in the corresponding [exposed variable table](MotorControl/commands.c). + +For example +* `g 0 12` will return the phase resistance of M0 +* `s 0 8 10000.0` will set the velocity limit on M0 to 10000 counts/s +* `g 1 3` will return the error status of M0 +* `g 1 7` will return the error status of M1 + +The error status corresponds to the [Error_t enum in low_level.h](MotorControl/low_level.h). + +Note that the links in this section are to a specific commits to make sure that the line numbers are accurate. That is, they don't link to the newest master, but to an old version. Please check the corresponding lines in the code you are using. This is especially important to get the correct indicies in the exposed variable tables, and the error enum values. + +#### Continous monitoring of variables +You can set up variables in monitoring slots, and then have them (or a subset of them) repeatedly printed upon request. Please see the code for this. diff --git a/Firmware/protocol.md b/Firmware/protocol.md new file mode 100644 index 00000000..1396c287 --- /dev/null +++ b/Firmware/protocol.md @@ -0,0 +1,69 @@ + +# ODrive Communication Protocol # + +Communicating with an ODrive consists of a series of endpoint operations. +An endpoint can theoretically be any kind data serialized in any way. +There is a default seralization implementation for POD types; for custom types +you must (de)seralize yourself. In the future we may provide a default seralizer +for stucts. +The available endpoints can be enumerated by reading the JSON from endpoint 0 +and can theoretically be different for each communication interface (they are not in practice). + +Each endpoint operation can send bytes to one endpoint (referenced by it's ID) +and at the same time receive bytes from the same endpoint. The semantics of +these payloads are specific to each endpoint's type, the name of which is +indicated in the JSON. + +For instance an int32 endpoint's input and output is a 4 byte little endian +representation. In general the convention for combined read/write requests is +_exchange_, i.e. the returned value is the old value. Custom endpoint handlers +may be non-compliant. + +There is a packet based version and a stream based variant of the protocol. Each +variant is employed as appropriate. For instance USB runs the packet based variant +by default while UART runs the stream based variant. + + +## Packet format ## +We will call the ODrive "server" and the PC "client". A request is a message +from the PC to the ODrive and a response is a message from the PC to the +ODrive. + +Each request-response transaction corresponds to a single endpoint operation. + +__Request__ + + - __Bytes 0, 1__ Sequence number, MSB = 0 + Currently the server does not care about ordering and does not filter resent messages. + - __Bytes 2, 3__ Endpoint ID + The IDs of all endpoints can be obtained from the JSON definition. The JSON definition can be obtained by reading from endpoint 0. + If (and only if) the MSB is set to 1 the client expects a response for this request. + - __Bytes 4, 5__ Expected response size + The number of bytes that should be returned to the client. If the client doesn't need any response data, it can set this value to 0. The operation will still be acknowledged if the + MSB in EndpointID is set. + - __Bytes 6 to N-3__ Payload + The length of the payload is determined by the total packet size. The format of the payload depends on the endpoint type. The endpoint type can be obtained from the JSON definition. + - __Bytes N-2, N-1__ + For endpoint 0: Protocol version (currently 1). A server shall ignore packets with other + values. + For all other endpoints: The CRC16 calculated over the JSON definition. The CRC16 init value is the protocol version (currently 1). A server shall ignore packets that set this field incorrectly. See protocol.hpp for CRC details. + +__Response__ + + - __Bytes 0, 1__ Sequence number, MSB = 1 + The sequence number of the request to which this is the response. + - __Bytes 2, 3__ Payload + The length of the payload tends to be equal to the number of expected bytes as indicated + in the request. The server must not expect the client to accept more bytes than it requested. + +## Stream format ## +The stream based format is just a wrapper for the packet format. + + - __Byte 0__ Sync byte `0xAA` + - __Bytes 1, 2__ Packet length + Currently both parties shall only emit and accept values of 0 through 127. + - __Bytes 3__ CRC8 of bytes 0 through 2 + See protocol.hpp for CRC details. + - __Bytes 4 to N-3__ Packet + - __Bytes N-2, N-1__ CRC16 + See protocol.hpp for CRC details. diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py index 7c2a22c2..10533a40 100755 --- a/tools/explore_odrive.py +++ b/tools/explore_odrive.py @@ -16,9 +16,9 @@ group = parser.add_mutually_exclusive_group() group.add_argument("-d", "--discover", metavar="CHANNELS", action="store", help="Automatically discover ODrives. Takes a comma-separated list (without spaces) " "to indicate which connection types should be considered. Possible values are " - "usb and serial. For example \"--discover=usb,serial\" indicates " + "usb and serial. For example \"--discover usb,serial\" indicates " "that USB and serial ports should be scanned for ODrives. " - "If none of the below options are specified, --discover=usb is assumed.") + "If none of the below options are specified, --discover usb is assumed.") group.add_argument("-u", "--usb", metavar="BUS:DEVICE", action="store", help="Specifies the USB port on which the device is connected. " "For example \"001:014\" means bus 001, device 014. The numbers can be obtained " diff --git a/tools/test_communication.py b/tools/test_communication.py index a05c5241..d6eddae5 100755 --- a/tools/test_communication.py +++ b/tools/test_communication.py @@ -11,9 +11,9 @@ def parse_args(): group.add_argument("-d", "--discover", metavar="CHANNELS", action="store", help="Automatically discover ODrives. Takes a comma-separated list (without spaces) " "to indicate which connection types should be considered. Possible values are " - "usb and serial. For example \"--discover=usb,serial\" indicates " + "usb and serial. For example \"--discover usb,serial\" indicates " "that USB and serial ports should be scanned for ODrives. " - "If none of the below options are specified, --discover=usb is assumed.") + "If none of the below options are specified, --discover usb is assumed.") group.add_argument("-u", "--usb", metavar="BUS:DEVICE", action="store", help="Specifies the USB port on which the device is connected. " "For example \"001:014\" means bus 001, device 014. The numbers can be obtained " From e982eb4bd9e49d4584e4729607397602470dc5c9 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 13 Nov 2017 23:26:12 +0100 Subject: [PATCH 026/155] fix doc formatting --- Firmware/README.md | 2 +- Firmware/protocol.md | 23 +++++++++++------------ 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/Firmware/README.md b/Firmware/README.md index ed6efe52..80489aa1 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -147,7 +147,7 @@ pip install pyusb pyserial prompt_toolkit * If 'Odrive V3.x' is not in the list of devices upon opening Zadig check 'List All Devices' from the options menu. Connecting to the Odrive board directly and not over a usb hub may also help. With the Odrive selected in the device list choose 'libusb-win32' from the target driver list and select the large 'install driver' button. 7. Run `./tools/demo.py` or `./tools/explore_odrive.py`. - `demo.py` is a very simple script which will make motor 0 turn back and forth. Take a look at the code if you want to control the ODrive yourself programatically. - - `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --discover usb`. + - `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --discover serial`. ### From Arduino diff --git a/Firmware/protocol.md b/Firmware/protocol.md index 1396c287..ce22164e 100644 --- a/Firmware/protocol.md +++ b/Firmware/protocol.md @@ -34,26 +34,25 @@ Each request-response transaction corresponds to a single endpoint operation. __Request__ - __Bytes 0, 1__ Sequence number, MSB = 0 - Currently the server does not care about ordering and does not filter resent messages. + - Currently the server does not care about ordering and does not filter resent messages. - __Bytes 2, 3__ Endpoint ID - The IDs of all endpoints can be obtained from the JSON definition. The JSON definition can be obtained by reading from endpoint 0. + - The IDs of all endpoints can be obtained from the JSON definition. The JSON definition can be obtained by reading from endpoint 0. If (and only if) the MSB is set to 1 the client expects a response for this request. - __Bytes 4, 5__ Expected response size - The number of bytes that should be returned to the client. If the client doesn't need any response data, it can set this value to 0. The operation will still be acknowledged if the + - The number of bytes that should be returned to the client. If the client doesn't need any response data, it can set this value to 0. The operation will still be acknowledged if the MSB in EndpointID is set. - __Bytes 6 to N-3__ Payload - The length of the payload is determined by the total packet size. The format of the payload depends on the endpoint type. The endpoint type can be obtained from the JSON definition. + - The length of the payload is determined by the total packet size. The format of the payload depends on the endpoint type. The endpoint type can be obtained from the JSON definition. - __Bytes N-2, N-1__ - For endpoint 0: Protocol version (currently 1). A server shall ignore packets with other - values. - For all other endpoints: The CRC16 calculated over the JSON definition. The CRC16 init value is the protocol version (currently 1). A server shall ignore packets that set this field incorrectly. See protocol.hpp for CRC details. + - For endpoint 0: Protocol version (currently 1). A server shall ignore packets with other values. + - For all other endpoints: The CRC16 calculated over the JSON definition. The CRC16 init value is the protocol version (currently 1). A server shall ignore packets that set this field incorrectly. See protocol.hpp for CRC details. __Response__ - __Bytes 0, 1__ Sequence number, MSB = 1 - The sequence number of the request to which this is the response. + - The sequence number of the request to which this is the response. - __Bytes 2, 3__ Payload - The length of the payload tends to be equal to the number of expected bytes as indicated + - The length of the payload tends to be equal to the number of expected bytes as indicated in the request. The server must not expect the client to accept more bytes than it requested. ## Stream format ## @@ -61,9 +60,9 @@ The stream based format is just a wrapper for the packet format. - __Byte 0__ Sync byte `0xAA` - __Bytes 1, 2__ Packet length - Currently both parties shall only emit and accept values of 0 through 127. + - Currently both parties shall only emit and accept values of 0 through 127. - __Bytes 3__ CRC8 of bytes 0 through 2 - See protocol.hpp for CRC details. + - See protocol.hpp for CRC details. - __Bytes 4 to N-3__ Packet - __Bytes N-2, N-1__ CRC16 - See protocol.hpp for CRC details. + - See protocol.hpp for CRC details. From 7166634d177160dc86d33dfa463709204330da2c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 13 Nov 2017 16:32:24 -0800 Subject: [PATCH 027/155] Update README.md --- README.md | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 9ed8752c..22b83ed3 100644 --- a/README.md +++ b/README.md @@ -3,8 +3,6 @@ This project is all about accurately driving brushless motors, for cheap. The aim is to make it possible to use inexpensive brushless motors in high performance robotics projects, like [this](https://www.youtube.com/watch?v=WT4E5nb3KtY). ## Getting Started -*References to hardware is with respect to v3.3. Other versions may still apply, but component designators may differ* - It is perfectly fine, and even recommended, to start testing with just a single motor and encoder. Make sure you have a good mechanical connection between the encdoer and the motor, slip can cause disasterous oscillations. All non-power I/O is 3.3V output and 5V tolerant on input, except: @@ -20,8 +18,9 @@ Wire up the encoder(s) to J4. The A,B phases are required, and the Z (index puls ![Image of ODrive all hooked up](https://docs.google.com/drawings/d/e/2PACX-1vTCD0P40Cd-wvD7Fl8UYEaxp3_UL81oI4qUVqrrCJPi6tkJeSs2rsffIXQRpdu6rNZs6-2mRKKYtILG/pub?w=1716&h=1281) -The currently supported command modes are USB and step/direction. +The currently supported command modes are USB, UART and step/direction. * If you are sending commands over USB, you can plug in a cable into the micro-USB port. +* If you are sending commands over UART, please see [Setting up UART](#setting-up-uart) * If you are using step/direction, please see [setting up step/direction](#setting-up-stepdirection) You can now: From 6f9257cee107e423941a8f10961f2f187e746ef2 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 14 Nov 2017 15:39:26 -0800 Subject: [PATCH 028/155] fw v2.1 patch notes: fix usb deadlock --- Firmware/CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 3d928d83..eae9f6dc 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,3 +1,6 @@ +## [0.2.1] - 2017-11-14 +### Fixed +* USB communication deadlock ## [0.2.0] - 2017-11-12 ### Added From 40afb9daf1430df61c4402ff90bc3d02ade1c8d9 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 15 Nov 2017 00:43:46 -0800 Subject: [PATCH 029/155] fix EXTI handler redefinition for v3.2 --- Firmware/CHANGELOG.md | 1 + .../Src/prev_board_ver/stm32f4xx_it_V3_2.c | 28 ------------------- Firmware/Src/stm32f4xx_it.c | 8 ++++++ 3 files changed, 9 insertions(+), 28 deletions(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index eae9f6dc..1280cb86 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,6 +1,7 @@ ## [0.2.1] - 2017-11-14 ### Fixed * USB communication deadlock +* EXTI handler redefiniton in V3.2 ## [0.2.0] - 2017-11-12 ### Added diff --git a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c b/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c index 6ea106f5..4e8bd2b2 100644 --- a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c +++ b/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c @@ -120,34 +120,6 @@ void SysTick_Handler(void) /* please refer to the startup file (startup_stm32f4xx.s). */ /******************************************************************************/ -/** -* @brief This function handles EXTI line2 interrupt. -*/ -void EXTI2_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI2_IRQn 0 */ - - /* USER CODE END EXTI2_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2); - /* USER CODE BEGIN EXTI2_IRQn 1 */ - - /* USER CODE END EXTI2_IRQn 1 */ -} - -/** -* @brief This function handles EXTI line4 interrupt. -*/ -void EXTI4_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI4_IRQn 0 */ - - /* USER CODE END EXTI4_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4); - /* USER CODE BEGIN EXTI4_IRQn 1 */ - - /* USER CODE END EXTI4_IRQn 1 */ -} - /** * @brief This function handles ADC1, ADC2 and ADC3 global interrupts. */ diff --git a/Firmware/Src/stm32f4xx_it.c b/Firmware/Src/stm32f4xx_it.c index a740268a..8bb4559e 100644 --- a/Firmware/Src/stm32f4xx_it.c +++ b/Firmware/Src/stm32f4xx_it.c @@ -318,5 +318,13 @@ void EXTI2_IRQHandler(void) HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2); } +/** +* @brief This function handles EXTI line4 interrupt. +*/ +void EXTI4_IRQHandler(void) +{ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4); +} + /* USER CODE END 1 */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ From 1ec18e8d0cfe3b90d59790f306ff0256c4c2b6c0 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 15 Nov 2017 01:05:18 -0800 Subject: [PATCH 030/155] save L R vals dispite out of range --- Firmware/CHANGELOG.md | 3 +++ Firmware/MotorControl/low_level.c | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 1280cb86..c89393f9 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -3,6 +3,9 @@ * USB communication deadlock * EXTI handler redefiniton in V3.2 +### Changed +* Resistance/inductance measurement now saved dispite errors, to allow debugging + ## [0.2.0] - 2017-11-12 ### Added * UART communication diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index c5904223..961d4f7b 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -684,11 +684,11 @@ static bool measure_phase_resistance(Motor_t* motor, float test_current, float m queue_voltage_timings(motor, 0.0f, 0.0f); float R = test_voltage / test_current; + motor->phase_resistance = R; if (fabs(test_voltage) == fabs(max_voltage) || R < 0.01f || R > 1.0f) { motor->error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; return false; } - motor->phase_resistance = R; return true; } @@ -726,12 +726,12 @@ static bool measure_phase_inductance(Motor_t* motor, float voltage_low, float vo float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); float L = v_L / dI_by_dt; + motor->phase_inductance = L; // TODO arbitrary values set for now if (L < 1e-6f || L > 500e-6f) { motor->error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; return false; } - motor->phase_inductance = L; return true; } From ea46728431cc8a36474afc77032da3368fd6f305 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 17 Nov 2017 04:46:03 -0800 Subject: [PATCH 031/155] GPIO 1,2 default to not initialized --- Firmware/MotorControl/commands.c | 10 ++++++++-- Firmware/MotorControl/commands.h | 1 + 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/commands.c b/Firmware/MotorControl/commands.c index 6978a091..2e0f5e52 100644 --- a/Firmware/MotorControl/commands.c +++ b/Firmware/MotorControl/commands.c @@ -20,7 +20,8 @@ SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_NONE; /* Private constant data -----------------------------------------------------*/ // TODO: make command to switch gpio_mode during run-time -static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx +static const GpioMode_t gpio_mode = GPIO_MODE_NONE; //GPIO 1,2 is not configured +// static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx // static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir static uint8_t* usb_buf; @@ -127,12 +128,17 @@ static void print_monitoring(int limit); /* Function implementations --------------------------------------------------*/ void init_communication() { switch (gpio_mode) { + case GPIO_MODE_NONE: + break; //do nothing case GPIO_MODE_UART: { SetGPIO12toUART(); } break; case GPIO_MODE_STEP_DIR: { SetGPIO12toStepDir(); - } + } break; + default: + //TODO: report error unexpected mode + break; } } diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 1cb5ad31..1c99096f 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -6,6 +6,7 @@ /* Exported types ------------------------------------------------------------*/ typedef enum { + GPIO_MODE_NONE, GPIO_MODE_UART, GPIO_MODE_STEP_DIR, } GpioMode_t; From 1d48659367776d7deade64b69c1b926388036249 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 17 Nov 2017 04:48:37 -0800 Subject: [PATCH 032/155] interrupt vectors no longer different between HW versions --- .../Src/prev_board_ver/stm32f4xx_it_V3_2.c | 168 ------------------ Firmware/Src/stm32f4xx_it.c | 5 - 2 files changed, 173 deletions(-) delete mode 100644 Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c diff --git a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c b/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c deleted file mode 100644 index 4e8bd2b2..00000000 --- a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c +++ /dev/null @@ -1,168 +0,0 @@ -/* External variables --------------------------------------------------------*/ -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern ADC_HandleTypeDef hadc1; -extern ADC_HandleTypeDef hadc2; -extern ADC_HandleTypeDef hadc3; - -/******************************************************************************/ -/* Cortex-M4 Processor Interruption and Exception Handlers */ -/******************************************************************************/ - -/** -* @brief This function handles Non maskable interrupt. -*/ -void NMI_Handler(void) -{ - /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ - - /* USER CODE END NonMaskableInt_IRQn 0 */ - /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ - - /* USER CODE END NonMaskableInt_IRQn 1 */ -} - -/** -* @brief This function handles Hard fault interrupt. -*/ -void HardFault_Handler(void) -{ - /* USER CODE BEGIN HardFault_IRQn 0 */ - - /* USER CODE END HardFault_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN HardFault_IRQn 1 */ - - /* USER CODE END HardFault_IRQn 1 */ -} - -/** -* @brief This function handles Memory management fault. -*/ -void MemManage_Handler(void) -{ - /* USER CODE BEGIN MemoryManagement_IRQn 0 */ - - /* USER CODE END MemoryManagement_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN MemoryManagement_IRQn 1 */ - - /* USER CODE END MemoryManagement_IRQn 1 */ -} - -/** -* @brief This function handles Pre-fetch fault, memory access fault. -*/ -void BusFault_Handler(void) -{ - /* USER CODE BEGIN BusFault_IRQn 0 */ - - /* USER CODE END BusFault_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN BusFault_IRQn 1 */ - - /* USER CODE END BusFault_IRQn 1 */ -} - -/** -* @brief This function handles Undefined instruction or illegal state. -*/ -void UsageFault_Handler(void) -{ - /* USER CODE BEGIN UsageFault_IRQn 0 */ - - /* USER CODE END UsageFault_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN UsageFault_IRQn 1 */ - - /* USER CODE END UsageFault_IRQn 1 */ -} - -/** -* @brief This function handles Debug monitor. -*/ -void DebugMon_Handler(void) -{ - /* USER CODE BEGIN DebugMonitor_IRQn 0 */ - - /* USER CODE END DebugMonitor_IRQn 0 */ - /* USER CODE BEGIN DebugMonitor_IRQn 1 */ - - /* USER CODE END DebugMonitor_IRQn 1 */ -} - -/** -* @brief This function handles System tick timer. -*/ -void SysTick_Handler(void) -{ - /* USER CODE BEGIN SysTick_IRQn 0 */ - - /* USER CODE END SysTick_IRQn 0 */ - HAL_IncTick(); - osSystickHandler(); - /* USER CODE BEGIN SysTick_IRQn 1 */ - - /* USER CODE END SysTick_IRQn 1 */ -} - -/******************************************************************************/ -/* STM32F4xx Peripheral Interrupt Handlers */ -/* Add here the Interrupt Handlers for the used peripherals. */ -/* For the available peripheral interrupt handler names, */ -/* please refer to the startup file (startup_stm32f4xx.s). */ -/******************************************************************************/ - -/** -* @brief This function handles ADC1, ADC2 and ADC3 global interrupts. -*/ -void ADC_IRQHandler(void) -{ - /* USER CODE BEGIN ADC_IRQn 0 */ - - // The HAL's ADC handling mechanism adds many clock cycles of overhead - // So we bypass it and handle the logic ourselves. - //@TODO add vbus meaasurement on adc1 here - ADC_IRQ_Dispatch(&hadc1, &vbus_sense_adc_cb); - ADC_IRQ_Dispatch(&hadc2, &pwm_trig_adc_cb); - ADC_IRQ_Dispatch(&hadc3, &pwm_trig_adc_cb); - - // Bypass HAL - return; - - /* USER CODE END ADC_IRQn 0 */ - HAL_ADC_IRQHandler(&hadc1); - HAL_ADC_IRQHandler(&hadc2); - HAL_ADC_IRQHandler(&hadc3); - /* USER CODE BEGIN ADC_IRQn 1 */ - - /* USER CODE END ADC_IRQn 1 */ -} - -/** -* @brief This function handles USB On The Go FS global interrupt. -*/ -void OTG_FS_IRQHandler(void) -{ - /* USER CODE BEGIN OTG_FS_IRQn 0 */ - - // Mask interrupt, and signal processing of interrupt by usb_cmd_thread - // The thread will re-enable the interrupt when all pending irqs are clear. - HAL_NVIC_DisableIRQ(OTG_FS_IRQn); - osSemaphoreRelease(sem_usb_irq); - // Bypass interrupt processing here - return; - - /* USER CODE END OTG_FS_IRQn 0 */ - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - /* USER CODE BEGIN OTG_FS_IRQn 1 */ - - /* USER CODE END OTG_FS_IRQn 1 */ -} \ No newline at end of file diff --git a/Firmware/Src/stm32f4xx_it.c b/Firmware/Src/stm32f4xx_it.c index 8bb4559e..8f6d6d42 100644 --- a/Firmware/Src/stm32f4xx_it.c +++ b/Firmware/Src/stm32f4xx_it.c @@ -43,10 +43,6 @@ typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected); void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback); -#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ -|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 -#include "prev_board_ver/stm32f4xx_it_V3_2.c" -#else /* USER CODE END 0 */ /* External variables --------------------------------------------------------*/ @@ -281,7 +277,6 @@ void OTG_FS_IRQHandler(void) } /* USER CODE BEGIN 1 */ -#endif void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback) { From 4ad21b8fbff906c5d092c028504a2874c4238acf Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 17 Nov 2017 04:51:39 -0800 Subject: [PATCH 033/155] update CHANGELOG.md --- Firmware/CHANGELOG.md | 7 +++++++ Firmware/Inc/main.h | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index c89393f9..a896ab25 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,3 +1,10 @@ +## [0.2.2] - 2017-11-17 +### Fixed +* Incorrect TIM14 interrupt mapping on board v3.2 caused hard-fault + +### Changed +* GPIO communication mode now defaults to NONE + ## [0.2.1] - 2017-11-14 ### Fixed * USB communication deadlock diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index 911d81fe..7a759bc8 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -53,7 +53,7 @@ /* USER CODE BEGIN Includes */ #define HW_VERSION_MAJOR 3 -#define HW_VERSION_MINOR 3 +#define HW_VERSION_MINOR 2 #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 From 7df6c19eb356860a3522d0701e5307743366665b Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 17 Nov 2017 04:54:36 -0800 Subject: [PATCH 034/155] revert acedental change of default HW version --- Firmware/Inc/main.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index 7a759bc8..911d81fe 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -53,7 +53,7 @@ /* USER CODE BEGIN Includes */ #define HW_VERSION_MAJOR 3 -#define HW_VERSION_MINOR 2 +#define HW_VERSION_MINOR 3 #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 From ee1a2d504ebea232c4e8f06b9e788a83b55205c6 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 20 Nov 2017 20:54:34 +0100 Subject: [PATCH 035/155] fix default GPIO mode --- Firmware/MotorControl/commands.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 3a8fd213..1670371a 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -22,7 +22,7 @@ //#define USB_PROTOCOL_NONE // Use GPIO 1/2 for step/dir input instead of UART -#define USE_GPIO_MODE_STEP_DIR +//#define USE_GPIO_MODE_STEP_DIR typedef enum { From a7b11f8c6db30af55302b8b0343bd9b601579208 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 20 Nov 2017 20:10:38 -0800 Subject: [PATCH 036/155] Update README.md --- Firmware/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index ec5d96f6..eebd5de8 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -105,7 +105,7 @@ After installing all of the above, open a Git Bash shell. Continue at section [B ### Flashing the firmware * **Make sure you have [configured the parameters first](#configuring-parameters)** -* Connect `SWD`, `SWC`, and `GND` on connector J2 to the programmer. +* Connect `GND`, `SWD`, and `SWC` on connector J2 to the programmer. Note: Always plug in `GND` first! * You need to power the board by only **ONE** of the following: VCC(3.3v), 5V, or the main power connection (the DC bus). The USB port (J1) does not power the board. * Run `make flash` in the root of this repository. From fefa091de22bc8ad00292ff476a0b87a0f1c709d Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 19:17:31 -0500 Subject: [PATCH 037/155] Handle current scaling factor for HW versions <= 3.3 --- Firmware/MotorControl/low_level.c | 16 ++++++++++++++-- Firmware/MotorControl/utils.h | 1 + 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 961d4f7b..97ee3a9b 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -39,6 +39,8 @@ float vbus_voltage = 12.0f; #define POLE_PAIRS 7 static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); +#define CURRENT_SCALING_FACTOR (1.31578947f) + // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { @@ -619,7 +621,12 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } else { ADCValue = HAL_ADC_GetValue(hadc); } - float current = phase_current_from_adcval(motor, ADCValue); + if(HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 3){ + float current = phase_current_from_adcval(motor, ADCValue)*CURRENT_SCALING_FACTOR; + } else { + float current = phase_current_from_adcval(motor, ADCValue); + } + if (current_meas_not_DC_CAL) { // ADC2 and ADC3 record the phB and phC currents concurrently, @@ -1293,7 +1300,12 @@ static void control_motor_loop(Motor_t* motor) { } // Current limiting - float Ilim = motor->current_control.current_lim; + if(HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 3){ + float Ilim = motor->current_control.current_lim*CURRENT_SCALING_FACTOR; + } else{ + float Ilim = motor->current_control.current_lim; + } + bool limited = false; if (Iq > Ilim) { limited = true; diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 542b7dd7..1acfc657 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -70,6 +70,7 @@ #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) +#define MACRO_CONSTRAIN(amt,low,high) (((amt)<(low)) ? (low) : ((amt > high) ? (high) : (amt))) // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform From 62be001ca29555283d50a4a231ce531d5e80253f Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 19:17:31 -0500 Subject: [PATCH 038/155] Revert "Handle current scaling factor for HW versions <= 3.3" This reverts commit fefa091de22bc8ad00292ff476a0b87a0f1c709d. --- Firmware/MotorControl/low_level.c | 16 ++-------------- Firmware/MotorControl/utils.h | 1 - 2 files changed, 2 insertions(+), 15 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 97ee3a9b..961d4f7b 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -39,8 +39,6 @@ float vbus_voltage = 12.0f; #define POLE_PAIRS 7 static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); -#define CURRENT_SCALING_FACTOR (1.31578947f) - // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { @@ -621,12 +619,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } else { ADCValue = HAL_ADC_GetValue(hadc); } - if(HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 3){ - float current = phase_current_from_adcval(motor, ADCValue)*CURRENT_SCALING_FACTOR; - } else { - float current = phase_current_from_adcval(motor, ADCValue); - } - + float current = phase_current_from_adcval(motor, ADCValue); if (current_meas_not_DC_CAL) { // ADC2 and ADC3 record the phB and phC currents concurrently, @@ -1300,12 +1293,7 @@ static void control_motor_loop(Motor_t* motor) { } // Current limiting - if(HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 3){ - float Ilim = motor->current_control.current_lim*CURRENT_SCALING_FACTOR; - } else{ - float Ilim = motor->current_control.current_lim; - } - + float Ilim = motor->current_control.current_lim; bool limited = false; if (Iq > Ilim) { limited = true; diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 1acfc657..542b7dd7 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -70,7 +70,6 @@ #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) -#define MACRO_CONSTRAIN(amt,low,high) (((amt)<(low)) ? (low) : ((amt > high) ? (high) : (amt))) // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform From 63d8a12f6ee1328e5695ff2a5aa29397337d1e24 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 20:48:43 -0500 Subject: [PATCH 039/155] Shunt conductance depends on HW verison --- Firmware/MotorControl/low_level.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 961d4f7b..04fa245f 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -39,6 +39,14 @@ float vbus_voltage = 12.0f; #define POLE_PAIRS 7 static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); +#if HW_VERSION_MAJOR == 3 + #if HW_VERSION_MINOR < 4 + #define SHUNT_RESISTANCE (666e-6) + #else + #define SHUNT_RESISTANCE (500e-6) + #endif +#endif + // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { @@ -83,7 +91,7 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] + .shunt_conductance = 1.0f/SHUNT_RESISTANCE, //[S] .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic @@ -175,7 +183,7 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] + .shunt_conductance = 1.0f/SHUNT_RESISTANCE, //[S] .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic From d9b13efac28a6486008ac8237dc9a994b2b9fcb6 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 20:49:09 -0500 Subject: [PATCH 040/155] Check for max current based on HW version --- Firmware/MotorControl/low_level.c | 10 +++++++++- Firmware/MotorControl/low_level.h | 1 + 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 04fa245f..3062fd60 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -144,6 +144,7 @@ Motor_t motors[] = { .calib_pos_threshold = 1.0f, .calib_vel_threshold = 1.0f, }, + .max_allowed_current = 0.0f, }, { // M1 .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t @@ -233,7 +234,8 @@ Motor_t motors[] = { .calib_anticogging = false, .calib_pos_threshold = 1.0f, .calib_vel_threshold = 1.0f, - } + }, + .max_allowed_current = 0.0f, } }; const int num_motors = sizeof(motors)/sizeof(motors[0]); @@ -402,6 +404,7 @@ static void DRV8301_setup(Motor_t* motor) { local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; switch (local_regs->Ctrl_Reg_2.GAIN) { + case DRV8301_ShuntAmpGain_10VpV: motor->phase_current_rev_gain = 1.0f/10.0f; break; @@ -416,6 +419,11 @@ static void DRV8301_setup(Motor_t* motor) { break; } + float margin = 0.95f; + float max_input = margin * 0.3f * motor->shunt_conductance; + float max_swing = margin * 1.6f * motor->shunt_conductance / motor->phase_current_rev_gain; + motor->max_allowed_current = MACRO_MIN(max_input, max_swing); + local_regs->SndCmd = true; DRV8301_writeData(gate_driver, local_regs); local_regs->RcvCmd = true; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 12eac1ba..4293f935 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -146,6 +146,7 @@ typedef struct { int timing_log_index; uint16_t timing_log[TIMING_LOG_SIZE]; Anticogging_t anticogging; + float max_allowed_current; } Motor_t; typedef struct{ From 4a868daaf09b5dd09ce56d6f63a3d550038e6d44 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 20:50:13 -0500 Subject: [PATCH 041/155] Fix GAIN --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 3062fd60..44488489 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -421,7 +421,7 @@ static void DRV8301_setup(Motor_t* motor) { float margin = 0.95f; float max_input = margin * 0.3f * motor->shunt_conductance; - float max_swing = margin * 1.6f * motor->shunt_conductance / motor->phase_current_rev_gain; + float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; motor->max_allowed_current = MACRO_MIN(max_input, max_swing); local_regs->SndCmd = true; From f115a74dde2a9ccb463307474d67d24430a70201 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 20:50:32 -0500 Subject: [PATCH 042/155] Clamp to the lowest of (current_lim) and (max_allowed_current) --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 44488489..79617b44 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -1309,7 +1309,7 @@ static void control_motor_loop(Motor_t* motor) { } // Current limiting - float Ilim = motor->current_control.current_lim; + float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->max_allowed_current); bool limited = false; if (Iq > Ilim) { limited = true; From 161651ae484dd5dfccda5370f8a6c9ba59406e02 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 22 Nov 2017 21:11:38 -0500 Subject: [PATCH 043/155] Move max_allowed_current to current_control struct --- Firmware/MotorControl/low_level.c | 10 +++++----- Firmware/MotorControl/low_level.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 79617b44..a4fe5379 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -104,6 +104,7 @@ Motor_t motors[] = { .final_v_alpha = 0.0f, .final_v_beta = 0.0f, .Iq = 0.0f, + .max_allowed_current = 0.0f, }, // .rotor_mode = ROTOR_MODE_SENSORLESS, // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, @@ -144,7 +145,6 @@ Motor_t motors[] = { .calib_pos_threshold = 1.0f, .calib_vel_threshold = 1.0f, }, - .max_allowed_current = 0.0f, }, { // M1 .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t @@ -197,6 +197,7 @@ Motor_t motors[] = { .final_v_alpha = 0.0f, .final_v_beta = 0.0f, .Iq = 0.0f, + .max_allowed_current = 0.0f, }, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { @@ -234,8 +235,7 @@ Motor_t motors[] = { .calib_anticogging = false, .calib_pos_threshold = 1.0f, .calib_vel_threshold = 1.0f, - }, - .max_allowed_current = 0.0f, + } } }; const int num_motors = sizeof(motors)/sizeof(motors[0]); @@ -422,7 +422,7 @@ static void DRV8301_setup(Motor_t* motor) { float margin = 0.95f; float max_input = margin * 0.3f * motor->shunt_conductance; float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; - motor->max_allowed_current = MACRO_MIN(max_input, max_swing); + motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); local_regs->SndCmd = true; DRV8301_writeData(gate_driver, local_regs); @@ -1309,7 +1309,7 @@ static void control_motor_loop(Motor_t* motor) { } // Current limiting - float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->max_allowed_current); + float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->current_control.max_allowed_current); bool limited = false; if (Iq > Ilim) { limited = true; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 4293f935..4806bbcb 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -71,6 +71,7 @@ typedef struct { float final_v_alpha; // [V] float final_v_beta; // [V] float Iq; + float max_allowed_current; } Current_control_t; typedef enum { @@ -146,7 +147,6 @@ typedef struct { int timing_log_index; uint16_t timing_log[TIMING_LOG_SIZE]; Anticogging_t anticogging; - float max_allowed_current; } Motor_t; typedef struct{ From 0d7d5647f6722ab6ed6eff0a0032e56fa9864be3 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 23 Nov 2017 11:46:09 -0500 Subject: [PATCH 044/155] Add pinout to Firmware readme --- Firmware/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/README.md b/Firmware/README.md index ec5d96f6..10323203 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -204,6 +204,8 @@ We also use a tool to generate the Makefile. The steps to do this are as follows * Press `Project -> Generate code` * You may need to let it download some drivers and such. +You will likely want the pinout for this process. It is available [here](https://docs.google.com/spreadsheets/d/1QXDCs1IRtUyG__M_9WruWOheywb-GhOwFtfPcHuN2Fg/edit#gid=404444347) + ## Setting up Eclipse development environment ### Install From 2880b65a1ea55454272cbd8d16aedd912c547117 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 23 Nov 2017 15:42:22 -0500 Subject: [PATCH 045/155] Create setEncoderCount function --- Firmware/MotorControl/low_level.c | 7 +++++++ Firmware/MotorControl/low_level.h | 2 ++ 2 files changed, 9 insertions(+) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 961d4f7b..6b6235da 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -1379,3 +1379,10 @@ void motor_thread(void const * argument) { } motor->thread_ready = false; } + + +void setEncoderCount(Motor_t* motor, uint16_t count){ + motor->encoder.encoder_state = count; + motor->motor_timer->Instance->CNT = count; + motor->encoder.pll_pos = count; +} \ No newline at end of file diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 12eac1ba..281465d1 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -172,6 +172,8 @@ void step_cb(uint16_t GPIO_Pin); void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); +void setEncoderCount(Motor_t* motor, uint16_t count); + bool anti_cogging_calibration(Motor_t* motor); //@TODO move motor thread to high level file From e82da352e0cb3ef5ae2418881b2b7722a0d0b72a Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 23 Nov 2017 15:48:40 -0500 Subject: [PATCH 046/155] CNT is 32 bit, so count should be too Also, explicitly cast count to a float for pll_pos --- Firmware/MotorControl/low_level.c | 6 +++--- Firmware/MotorControl/low_level.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 6b6235da..42b99bca 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -1380,9 +1380,9 @@ void motor_thread(void const * argument) { motor->thread_ready = false; } - -void setEncoderCount(Motor_t* motor, uint16_t count){ +/** Function that sets the current encoder count to a desired 32-bit value. */ +void setEncoderCount(Motor_t* motor, uint32_t count){ motor->encoder.encoder_state = count; motor->motor_timer->Instance->CNT = count; - motor->encoder.pll_pos = count; + motor->encoder.pll_pos = (float)count; } \ No newline at end of file diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 281465d1..6512a719 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -172,7 +172,7 @@ void step_cb(uint16_t GPIO_Pin); void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); -void setEncoderCount(Motor_t* motor, uint16_t count); +void setEncoderCount(Motor_t* motor, uint32_t count); bool anti_cogging_calibration(Motor_t* motor); From cd2de216a55886d1f0a7d0f4d2f2303902ac1fad Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 24 Nov 2017 14:41:08 -0800 Subject: [PATCH 047/155] fix float literals, safer margin, extra instruction --- Firmware/MotorControl/low_level.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index a4fe5379..e07ff2d6 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -41,9 +41,9 @@ static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_C #if HW_VERSION_MAJOR == 3 #if HW_VERSION_MINOR < 4 - #define SHUNT_RESISTANCE (666e-6) + #define SHUNT_RESISTANCE (666e-6f) #else - #define SHUNT_RESISTANCE (500e-6) + #define SHUNT_RESISTANCE (500e-6f) #endif #endif @@ -401,10 +401,12 @@ static void DRV8301_setup(Motor_t* motor) { local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; // 20V/V on 500uOhm gives a range of +/- 150A // 40V/V on 500uOhm gives a range of +/- 75A + // 20V/V on 666uOhm gives a range of +/- 110A + // 40V/V on 666uOhm gives a range of +/- 55A local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: motor->phase_current_rev_gain = 1.0f/10.0f; break; @@ -419,7 +421,7 @@ static void DRV8301_setup(Motor_t* motor) { break; } - float margin = 0.95f; + float margin = 0.90f; float max_input = margin * 0.3f * motor->shunt_conductance; float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); From 638b829d9744c72e778fdcbdf5f82b3d0f9a5f66 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 24 Nov 2017 16:14:34 -0800 Subject: [PATCH 048/155] fine tune current sense adjustment --- Firmware/MotorControl/low_level.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index e07ff2d6..7b00312c 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -40,8 +40,8 @@ float vbus_voltage = 12.0f; static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); #if HW_VERSION_MAJOR == 3 - #if HW_VERSION_MINOR < 4 - #define SHUNT_RESISTANCE (666e-6f) + #if HW_VERSION_MINOR <= 3 + #define SHUNT_RESISTANCE (675e-6f) #else #define SHUNT_RESISTANCE (500e-6f) #endif From 700da0d4383df8b1728c36100b582ce7a998efbd Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 24 Nov 2017 16:27:30 -0800 Subject: [PATCH 049/155] my editor made the comments lined up all by itself. Nice! --- Firmware/CHANGELOG.md | 8 ++ Firmware/MotorControl/low_level.c | 163 +++++++++++++++--------------- 2 files changed, 90 insertions(+), 81 deletions(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index a896ab25..a4504408 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,3 +1,11 @@ +## [Unreleased] + +### Added +* Protection from user setting current_lim higher than is measurable + +### Changed +* Shunt resistance values for v3.3 and earlier to include extra resistance of PCB + ## [0.2.2] - 2017-11-17 ### Fixed * Incorrect TIM14 interrupt mapping on board v3.2 caused hard-fault diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 7b00312c..d878da7e 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -50,32 +50,33 @@ static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_C // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { - { // M0 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration + { + // M0 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] + .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, // .vel_setpoint = 800.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, .thread_ready = false, .enable_control = true, .do_calibration = true, .calibration_ok = false, .motor_timer = &htim1, - .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2}, + .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, .control_deadline = TIM_1_8_PERIOD_CLOCKS, .last_cpu_time = 0, .current_meas = {0.0f, 0.0f}, @@ -91,13 +92,15 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { - // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + // Read out max_allowed_current to see max supported value for current_lim. + // You can change DRV8301_ShuntAmpGain to get a different range. + // .current_lim = 75.0f, //[A] + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, .v_current_control_integral_q = 0.0f, .Ibus = 0.0f, @@ -110,30 +113,28 @@ Motor_t motors[] = { // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { - .encoder_timer = &htim3, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] + .encoder_timer = &htim3, .encoder_offset = 0, .encoder_state = 0, + .motor_dir = 0, // set by calib_enc_offset + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] }, .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] }, .timing_log_index = 0, .timing_log = {0}, @@ -146,30 +147,30 @@ Motor_t motors[] = { .calib_vel_threshold = 1.0f, }, }, - { // M1 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration + { // M1 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] + .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, .thread_ready = false, .enable_control = true, .do_calibration = true, .calibration_ok = false, .motor_timer = &htim8, - .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2}, - .control_deadline = (3*TIM_1_8_PERIOD_CLOCKS)/2, + .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, + .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, .last_cpu_time = 0, .current_meas = {0.0f, 0.0f}, .DC_calib = {0.0f, 0.0f}, @@ -184,13 +185,15 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { - // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + // Read out max_allowed_current to see max supported value for current_lim. + // You can change DRV8301_ShuntAmpGain to get a different range. + // .current_lim = 75.0f, //[A] + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, .v_current_control_integral_q = 0.0f, .Ibus = 0.0f, @@ -201,30 +204,28 @@ Motor_t motors[] = { }, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { - .encoder_timer = &htim4, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] + .encoder_timer = &htim4, .encoder_offset = 0, .encoder_state = 0, + .motor_dir = 0, // set by calib_enc_offset + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] }, .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] }, .timing_log_index = 0, .timing_log = {0}, From 57a34edd9a42e40aeb8a7af39f7ae18bda3a7084 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 24 Nov 2017 16:44:29 -0800 Subject: [PATCH 050/155] autoformat low_level.c --- Firmware/MotorControl/low_level.c | 323 ++++++++++++++---------------- 1 file changed, 154 insertions(+), 169 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index d878da7e..6e919a71 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -3,27 +3,27 @@ // Because of broken cmsis_os.h, we need to include arm_math first, // otherwise chip specific defines are ommited #include -#include // Sets up the correct chip specifc defines required by arm_math +#include // Sets up the correct chip specifc defines required by arm_math #define ARM_MATH_CM4 #include #include +#include +#include #include #include -#include -#include -#include -#include #include -#include +#include +#include #include +#include #include /* Private defines -----------------------------------------------------------*/ -#define STANDALONE_MODE // Drive operates without USB communication +#define STANDALONE_MODE // Drive operates without USB communication // #define DEBUG_PRINT /* Private macros ------------------------------------------------------------*/ @@ -35,16 +35,16 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (600*4) +#define ENCODER_CPR (600 * 4) #define POLE_PAIRS 7 static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); #if HW_VERSION_MAJOR == 3 - #if HW_VERSION_MINOR <= 3 - #define SHUNT_RESISTANCE (675e-6f) - #else - #define SHUNT_RESISTANCE (500e-6f) - #endif +#if HW_VERSION_MINOR <= 3 +#define SHUNT_RESISTANCE (675e-6f) +#else +#define SHUNT_RESISTANCE (500e-6f) +#endif #endif // TODO: Migrate to C++, clearly we are actually doing object oriented code here... @@ -239,7 +239,7 @@ Motor_t motors[] = { } } }; -const int num_motors = sizeof(motors)/sizeof(motors[0]); +const int num_motors = sizeof(motors) / sizeof(motors[0]); /* Private constant data -----------------------------------------------------*/ static const float one_by_sqrt3 = 0.57735026919f; @@ -248,7 +248,7 @@ static const float current_meas_period = CURRENT_MEAS_PERIOD; static const int current_meas_hz = CURRENT_MEAS_HZ; /* Private variables ---------------------------------------------------------*/ -static float brake_resistance = 0.47f; // [ohm] +static float brake_resistance = 0.47f; // [ohm] /* Private function prototypes -----------------------------------------------*/ // Command Handling @@ -262,7 +262,7 @@ static void DRV8301_setup(Motor_t* motor); static void start_adc_pwm(); static void start_pwm(TIM_HandleTypeDef* htim); static void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, - uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset); + uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset); // IRQ Callbacks (are all public) // Measurement and calibrationa static bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage); @@ -284,7 +284,6 @@ static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); static void control_motor_loop(Motor_t* motor); // Motor thread (is public) - /* Function implementations --------------------------------------------------*/ //-------------------------------- @@ -292,8 +291,6 @@ static void control_motor_loop(Motor_t* motor); // TODO move to different file //-------------------------------- - - void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) { motor->pos_setpoint = pos_setpoint; motor->vel_setpoint = vel_feed_forward; @@ -334,7 +331,7 @@ static uint16_t check_timing(Motor_t* motor) { timing = TIM_1_8_PERIOD_CLOCKS + delta; } - if(++(motor->timing_log_index) == TIMING_LOG_SIZE){ + if (++(motor->timing_log_index) == TIMING_LOG_SIZE) { motor->timing_log_index = 0; } motor->timing_log[motor->timing_log_index] = timing; @@ -342,7 +339,7 @@ static uint16_t check_timing(Motor_t* motor) { return timing; } -static void global_fault(int error){ +static void global_fault(int error) { // Disable motors NOW! for (int i = 0; i < num_motors; ++i) { __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motors[i].motor_timer); @@ -358,14 +355,13 @@ static void global_fault(int error){ } static float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) { - int adcval_bal = (int)ADCValue - (1<<11); - float amp_out_volt = (3.3f/(float)(1<<12)) * (float)adcval_bal; + int adcval_bal = (int)ADCValue - (1 << 11); + float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; float shunt_volt = amp_out_volt * motor->phase_current_rev_gain; float current = shunt_volt * motor->shunt_conductance; return current; } - //-------------------------------- // Initalisation //-------------------------------- @@ -390,50 +386,50 @@ void init_motor_control() { // Set up the gate drivers static void DRV8301_setup(Motor_t* motor) { - DRV8301_Obj* gate_driver = &motor->gate_driver; - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; + DRV8301_Obj* gate_driver = &motor->gate_driver; + DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - DRV8301_enable(gate_driver); - DRV8301_setupSpi(gate_driver, local_regs); + DRV8301_enable(gate_driver); + DRV8301_setupSpi(gate_driver, local_regs); - // TODO we can use reporting only if we actually wire up the nOCTW pin - local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - // Overcurrent set to approximately 150A at 100degC. This may need tweaking. - local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - // 20V/V on 500uOhm gives a range of +/- 150A - // 40V/V on 500uOhm gives a range of +/- 75A - // 20V/V on 666uOhm gives a range of +/- 110A - // 40V/V on 666uOhm gives a range of +/- 55A - local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; - // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; + // TODO we can use reporting only if we actually wire up the nOCTW pin + local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; + // Overcurrent set to approximately 150A at 100degC. This may need tweaking. + local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + // 20V/V on 500uOhm gives a range of +/- 150A + // 40V/V on 500uOhm gives a range of +/- 75A + // 20V/V on 666uOhm gives a range of +/- 110A + // 40V/V on 666uOhm gives a range of +/- 55A + local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; - switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: - motor->phase_current_rev_gain = 1.0f/10.0f; - break; - case DRV8301_ShuntAmpGain_20VpV: - motor->phase_current_rev_gain = 1.0f/20.0f; - break; - case DRV8301_ShuntAmpGain_40VpV: - motor->phase_current_rev_gain = 1.0f/40.0f; - break; - case DRV8301_ShuntAmpGain_80VpV: - motor->phase_current_rev_gain = 1.0f/80.0f; - break; - } + switch (local_regs->Ctrl_Reg_2.GAIN) { + case DRV8301_ShuntAmpGain_10VpV: + motor->phase_current_rev_gain = 1.0f / 10.0f; + break; + case DRV8301_ShuntAmpGain_20VpV: + motor->phase_current_rev_gain = 1.0f / 20.0f; + break; + case DRV8301_ShuntAmpGain_40VpV: + motor->phase_current_rev_gain = 1.0f / 40.0f; + break; + case DRV8301_ShuntAmpGain_80VpV: + motor->phase_current_rev_gain = 1.0f / 80.0f; + break; + } - float margin = 0.90f; - float max_input = margin * 0.3f * motor->shunt_conductance; - float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; - motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); + float margin = 0.90f; + float max_input = margin * 0.3f * motor->shunt_conductance; + float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; + motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); - local_regs->SndCmd = true; - DRV8301_writeData(gate_driver, local_regs); - local_regs->RcvCmd = true; - DRV8301_readData(gate_driver, local_regs); + local_regs->SndCmd = true; + DRV8301_writeData(gate_driver, local_regs); + local_regs->RcvCmd = true; + DRV8301_readData(gate_driver, local_regs); } -static void start_adc_pwm(){ +static void start_adc_pwm() { // Enable ADC and interrupts __HAL_ADC_ENABLE(&hadc1); __HAL_ADC_ENABLE(&hadc2); @@ -453,7 +449,7 @@ static void start_adc_pwm(){ start_pwm(&htim1); start_pwm(&htim8); // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS/2 - 1*128); + sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); // Motor output starts in the disabled state __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); @@ -461,14 +457,14 @@ static void start_adc_pwm(){ // Start brake resistor PWM in floating output configuration htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS+1; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); } -static void start_pwm(TIM_HandleTypeDef* htim){ +static void start_pwm(TIM_HandleTypeDef* htim) { // Init PWM - int half_load = TIM_1_8_PERIOD_CLOCKS/2; + int half_load = TIM_1_8_PERIOD_CLOCKS / 2; htim->Instance->CCR1 = half_load; htim->Instance->CCR2 = half_load; htim->Instance->CCR3 = half_load; @@ -486,8 +482,7 @@ static void start_pwm(TIM_HandleTypeDef* htim){ } static void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, - uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { - + uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { // Store intial timer configs uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); @@ -532,7 +527,6 @@ static void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, htim_b->Instance->BDTR |= MOE_store_b; } - //-------------------------------- // IRQ Callbacks //-------------------------------- @@ -542,30 +536,30 @@ void step_cb(uint16_t GPIO_Pin) { GPIO_PinState dir_pin; float dir; switch (GPIO_Pin) { - case GPIO_1_Pin: - //M0 stepped - if (motors[0].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[0].pos_setpoint += dir * motors[0].counts_per_step; - } - break; - case GPIO_3_Pin: - //M1 stepped - if (motors[1].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[1].pos_setpoint += dir * motors[1].counts_per_step; - } - break; - default: - global_fault(ERROR_UNEXPECTED_STEP_SRC); - break; + case GPIO_1_Pin: + //M0 stepped + if (motors[0].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[0].pos_setpoint += dir * motors[0].counts_per_step; + } + break; + case GPIO_3_Pin: + //M1 stepped + if (motors[1].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[1].pos_setpoint += dir * motors[1].counts_per_step; + } + break; + default: + global_fault(ERROR_UNEXPECTED_STEP_SRC); + break; } } void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * 11.0f / (float)(1<<12); + static const float voltage_scale = 3.3f * 11.0f / (float)(1 << 12); // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); vbus_voltage = ADCValue * voltage_scale; @@ -574,11 +568,11 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. // TODO: Document how the phasing is done, link to timing diagram void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - #define calib_tau 0.2f //@TOTO make more easily configurable +#define calib_tau 0.2f //@TOTO make more easily configurable static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; // Ensure ADCs are expected ones to simplify the logic below - if (!(hadc == &hadc2 || hadc == &hadc3)){ + if (!(hadc == &hadc2 || hadc == &hadc3)) { global_fault(ERROR_ADC_FAILED); return; }; @@ -589,7 +583,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // If we are counting down, we just sampled in SVM vector 7, with zero current Motor_t* motor = injected ? &motors[0] : &motors[1]; bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; - + bool current_meas_not_DC_CAL; if (motor == &motors[1] && counting_down) { // We are measuring M1 DC_CAL here @@ -667,19 +661,18 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } } - //-------------------------------- // Measurement and calibration //-------------------------------- // TODO check Ibeta balance to verify good motor connection static bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { - static const float kI = 10.0f; //[(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + static const float kI = 10.0f; //[(V/s)/A] + static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s float test_voltage = 0.0f; for (int i = 0; i < num_test_cycles; ++i) { osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); - if (evt.status != osEventSignal){ + if (evt.status != osEventSignal) { motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; return false; } @@ -693,7 +686,7 @@ static bool measure_phase_resistance(Motor_t* motor, float test_current, float m // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_PHASE_RESISTANCE_TIMING; return false; } @@ -729,7 +722,7 @@ static bool measure_phase_inductance(Motor_t* motor, float voltage_low, float vo // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_PHASE_INDUCTANCE_TIMING; return false; } @@ -744,7 +737,7 @@ static bool measure_phase_inductance(Motor_t* motor, float voltage_low, float vo // However, the discretisation in the current control loop inverts the same discrepancy float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); float L = v_L / dI_by_dt; - + motor->phase_inductance = L; // TODO arbitrary values set for now if (L < 1e-6f || L > 500e-6f) { @@ -759,15 +752,15 @@ static bool measure_phase_inductance(Motor_t* motor, float voltage_low, float vo static bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { static const float start_lock_duration = 1.0f; static const int num_steps = 1024; - static const float dt_step = 1.0f/500.0f; + static const float dt_step = 1.0f / 500.0f; static const float scan_range = 4.0f * M_PI; - const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) + const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; int32_t encvaluesum = 0; // go to encoder zero phase for start_lock_duration to get ready to scan - for (int i = 0; i < start_lock_duration*current_meas_hz; ++i) { + for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; @@ -776,13 +769,13 @@ static bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } // scan forwards for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { - for (int i = 0; i < dt_step*(float)current_meas_hz; ++i) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; @@ -801,13 +794,13 @@ static bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } // scan backwards for (float ph = scan_range / 2.0f; ph > -scan_range / 2.0f; ph -= step_size) { - for (int i = 0; i < dt_step*(float)current_meas_hz; ++i) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; @@ -818,7 +811,7 @@ static bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { return true; } -static bool motor_calibration(Motor_t* motor){ +static bool motor_calibration(Motor_t* motor) { motor->calibration_ok = false; motor->error = ERROR_NO_ERROR; @@ -835,18 +828,18 @@ static bool motor_calibration(Motor_t* motor){ if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) return false; } - + // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] + float current_control_bandwidth = 1000.0f; // [rad/s] motor->current_control.p_gain = current_control_bandwidth * motor->phase_inductance; float plant_pole = motor->phase_resistance / motor->phase_inductance; motor->current_control.i_gain = plant_pole * motor->current_control.p_gain; // Calculate encoder pll gains - float encoder_pll_bandwidth = 1000.0f; // [rad/s] + float encoder_pll_bandwidth = 1000.0f; // [rad/s] motor->encoder.pll_kp = 2.0f * encoder_pll_bandwidth; // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)){ + if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)) { motor->error = ERROR_CALIBRATION_TIMING; return false; } @@ -871,8 +864,8 @@ static bool motor_calibration(Motor_t* motor){ bool anti_cogging_calibration(Motor_t* motor) { if (motor->anticogging.calib_anticogging && motor->anticogging.cogging_map != NULL) { float pos_err = motor->anticogging.index - motor->encoder.pll_pos; - if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && - fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { + if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && + fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { motor->anticogging.cogging_map[motor->anticogging.index++] = motor->vel_integrator_current; } if (motor->anticogging.index < ENCODER_CPR) { @@ -893,18 +886,17 @@ bool anti_cogging_calibration(Motor_t* motor) { // Test functions //-------------------------------- -__attribute__((unused)) -static void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { +__attribute__((unused)) static void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { for (;;) { for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_SCAN_MOTOR_TIMING; return; } @@ -913,8 +905,7 @@ static void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude } //TODO integrate as mode in main control loop -__attribute__((unused)) -static void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { +__attribute__((unused)) static void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { for (;;) { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); update_rotor(motor); @@ -922,26 +913,24 @@ static void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); float s = arm_sin_f32(phase); - float v_alpha = c*v_d - s*v_q; - float v_beta = c*v_q + s*v_d; + float v_alpha = c * v_d - s * v_q; + float v_beta = c * v_q + s * v_d; queue_voltage_timings(motor, v_alpha, v_beta); // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_FOC_VOLTAGE_TIMING; return; } } } - //-------------------------------- // Main motor control //-------------------------------- static void update_rotor(Motor_t* motor) { - switch (motor->rotor_mode) { case ROTOR_MODE_ENCODER: case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: { @@ -970,11 +959,10 @@ static void update_rotor(Motor_t* motor) { encoder->pll_pos += current_meas_period * encoder->pll_kp * delta_pos; encoder->pll_vel += current_meas_period * encoder->pll_ki * delta_pos; } - // Drop through to sensorless if also testing - if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - break; + // Drop through to sensorless if also testing + if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) + break; case ROTOR_MODE_SENSORLESS: { - // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf // In particular, equation 8 (and by extension eqn 4 and 6). @@ -989,8 +977,7 @@ static void update_rotor(Motor_t* motor) { // Clarke transform float I_alpha_beta[2] = { -motor->current_meas.phB - motor->current_meas.phC, - one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC) - }; + one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC)}; // alpha-beta vector operations float eta[2]; @@ -1054,8 +1041,8 @@ static void update_rotor(Motor_t* motor) { } break; default: - //TODO error handling - break; + //TODO error handling + break; } } @@ -1075,9 +1062,9 @@ static bool using_sensorless(Motor_t* motor) { } static float get_rotor_phase(Motor_t* motor) { - if (using_encoder(motor)) + if (using_encoder(motor)) return motor->encoder.phase; - else if (using_sensorless(motor)) + else if (using_sensorless(motor)) return motor->sensorless.phase; else //TODO error handling @@ -1085,9 +1072,9 @@ static float get_rotor_phase(Motor_t* motor) { } static float get_pll_vel(Motor_t* motor) { - if (using_encoder(motor)) + if (using_encoder(motor)) return motor->encoder.pll_vel; - else if (using_sensorless(motor)) + else if (using_sensorless(motor)) return motor->sensorless.pll_vel; else //TODO error handling @@ -1111,7 +1098,6 @@ static bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { } static bool spin_up_sensorless(Motor_t* motor) { - static const float ramp_up_time = 0.4f; static const float ramp_up_distance = 4 * M_PI; float ramp_step = current_meas_period / ramp_up_time; @@ -1124,7 +1110,7 @@ static bool spin_up_sensorless(Motor_t* motor) { for (float x = 0.0f; x < 1.0f; x += ramp_step) { phase = wrap_pm_pi(ramp_up_distance * x); I_mag = motor->sensorless.spin_up_current * x; - if(!spin_up_timestep(motor, phase, I_mag)) + if (!spin_up_timestep(motor, phase, I_mag)) return false; } @@ -1132,7 +1118,7 @@ static bool spin_up_sensorless(Motor_t* motor) { while (vel < motor->sensorless.spin_up_target_vel) { vel += motor->sensorless.spin_up_acceleration * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - if(!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) + if (!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) return false; } @@ -1162,7 +1148,7 @@ static void update_brake_current(float brake_current) { // To avoid race condition, first reset timings to safe state // ch3 is low side, ch4 is high side htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS+1; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; htim2.Instance->CCR3 = low_off; htim2.Instance->CCR4 = high_on; } @@ -1193,8 +1179,8 @@ static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); float s = arm_sin_f32(phase); - float Id = c*Ialpha + s*Ibeta; - float Iq = c*Ibeta - s*Ialpha; + float Id = c * Ialpha + s * Ibeta; + float Iq = c * Ibeta - s * Ialpha; // Current error float Ierr_d = Id_des - Id; @@ -1212,9 +1198,8 @@ static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Vector modulation saturation, lock integrator if saturated // TODO make maximum modulation configurable - float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f/sqrtf(mod_d*mod_d + mod_q*mod_q); - if (mod_scalefactor < 1.0f) - { + float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); + if (mod_scalefactor < 1.0f) { mod_d *= mod_scalefactor; mod_q *= mod_scalefactor; // TODO make decayfactor configurable @@ -1233,17 +1218,17 @@ static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Above check doesn't work if last motor is executing voltage control // TODO trigger this update in control_motor_loop instead, // and make voltage control a control mode in it. - float Ibus_sum = 0.0f; - for (int i = 0; i < num_motors; ++i) { - Ibus_sum += motors[i].current_control.Ibus; - } - // Note: function will clip negative values to 0.0f - update_brake_current(-Ibus_sum); + float Ibus_sum = 0.0f; + for (int i = 0; i < num_motors; ++i) { + Ibus_sum += motors[i].current_control.Ibus; + } + // Note: function will clip negative values to 0.0f + update_brake_current(-Ibus_sum); // } // Inverse park transform - float mod_alpha = c*mod_d - s*mod_q; - float mod_beta = c*mod_q + s*mod_d; + float mod_alpha = c * mod_d - s * mod_q; + float mod_beta = c * mod_q + s * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl->final_v_alpha = mod_to_V * mod_alpha; @@ -1254,7 +1239,7 @@ static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_FOC_TIMING; return false; } @@ -1263,12 +1248,12 @@ static bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { static void control_motor_loop(Motor_t* motor) { while (motor->enable_control) { - if(osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal){ + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; break; } update_rotor(motor); - anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking + anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking // Position control // TODO Decide if we want to use encoder or pll position here @@ -1284,7 +1269,7 @@ static void control_motor_loop(Motor_t* motor) { // Velocity limiting float vel_lim = motor->vel_limit; - if (vel_des > vel_lim) vel_des = vel_lim; + if (vel_des > vel_lim) vel_des = vel_lim; if (vel_des < -vel_lim) vel_des = -vel_lim; // Velocity control @@ -1293,12 +1278,12 @@ static void control_motor_loop(Motor_t* motor) { // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == ENCODER_CPR - 1) - if(motor->anticogging.use_anticogging){ + if (motor->anticogging.use_anticogging) { Iq += motor->anticogging.cogging_map[mod(motor->encoder.pll_pos, ENCODER_CPR)]; } float v_err = vel_des - get_pll_vel(motor); - if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { Iq += motor->vel_gain * v_err; } @@ -1324,7 +1309,7 @@ static void control_motor_loop(Motor_t* motor) { } // Velocity integrator (behaviour dependent on limiting) - if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL ) { + if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL) { // reset integral if not in use motor->vel_integrator_current = 0.0f; } else { @@ -1338,8 +1323,8 @@ static void control_motor_loop(Motor_t* motor) { motor->current_control.Iq = Iq; // Execute current command - if(!FOC_current(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_current + if (!FOC_current(motor, 0.0f, Iq)) { + break; // in case of error exit loop, motor->error has been set by FOC_current } } @@ -1352,13 +1337,13 @@ static void control_motor_loop(Motor_t* motor) { // Motor thread //-------------------------------- -void motor_thread(void const * argument) { +void motor_thread(void const* argument) { Motor_t* motor = (Motor_t*)argument; // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f - motor->anticogging.cogging_map = (float*)malloc(ENCODER_CPR*sizeof(float)); - if(motor->anticogging.cogging_map != NULL){ - for(int i = 0; i < ENCODER_CPR; i++){ + motor->anticogging.cogging_map = (float*)malloc(ENCODER_CPR * sizeof(float)); + if (motor->anticogging.cogging_map != NULL) { + for (int i = 0; i < ENCODER_CPR; i++) { motor->anticogging.cogging_map[i] = 0.0f; } } @@ -1368,9 +1353,9 @@ void motor_thread(void const * argument) { for (;;) { if (motor->do_calibration) { - __HAL_TIM_MOE_ENABLE(motor->motor_timer);// enable pwm outputs + __HAL_TIM_MOE_ENABLE(motor->motor_timer); // enable pwm outputs motor_calibration(motor); - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor->motor_timer);// disables pwm outputs + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor->motor_timer); // disables pwm outputs motor->do_calibration = false; } @@ -1387,12 +1372,12 @@ void motor_thread(void const * argument) { __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor->motor_timer); motor->enable_step_dir = false; - if(motor->enable_control){ // if control is still enabled, we exited because of error + if (motor->enable_control) { // if control is still enabled, we exited because of error motor->calibration_ok = false; motor->enable_control = false; } } - + queue_voltage_timings(motor, 0.0f, 0.0f); osDelay(100); } From 48773e7e8d5d200fa189fbcb942d8e901a823322 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sun, 26 Nov 2017 00:44:10 -0500 Subject: [PATCH 051/155] Protect against race conditions while setting the encoder count --- Firmware/MotorControl/low_level.c | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 42b99bca..fa99ab27 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -1382,7 +1382,20 @@ void motor_thread(void const * argument) { /** Function that sets the current encoder count to a desired 32-bit value. */ void setEncoderCount(Motor_t* motor, uint32_t count){ + bool handlerMode = false; + UBaseType_t uxSavedInterruptStatus; + if(inHandlerMode()) { + handlerMode = true; + uxSavedInterruptStatus = taskENTER_CRITICAL_FROM_ISR(); + } else + taskENTER_CRITICAL(); + motor->encoder.encoder_state = count; motor->motor_timer->Instance->CNT = count; motor->encoder.pll_pos = (float)count; + + if(handlerMode){ + taskEXIT_CRITICAL_FROM_ISR(uxSavedInterruptStatus); + } else + taskEXIT_CRITICAL(); } \ No newline at end of file From 18d2418b61546cb237dc336232b7737309627abe Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sun, 26 Nov 2017 01:32:56 -0500 Subject: [PATCH 052/155] Fix IRQ handling method --- Firmware/MotorControl/low_level.c | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index fa99ab27..a8b78ed1 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -1382,20 +1382,10 @@ void motor_thread(void const * argument) { /** Function that sets the current encoder count to a desired 32-bit value. */ void setEncoderCount(Motor_t* motor, uint32_t count){ - bool handlerMode = false; - UBaseType_t uxSavedInterruptStatus; - if(inHandlerMode()) { - handlerMode = true; - uxSavedInterruptStatus = taskENTER_CRITICAL_FROM_ISR(); - } else - taskENTER_CRITICAL(); - + uint32_t prim = __get_PRIMASK(); + __disable_irq(); motor->encoder.encoder_state = count; motor->motor_timer->Instance->CNT = count; motor->encoder.pll_pos = (float)count; - - if(handlerMode){ - taskEXIT_CRITICAL_FROM_ISR(uxSavedInterruptStatus); - } else - taskEXIT_CRITICAL(); + __set_PRIMASK(prim); } \ No newline at end of file From 9309077b8579af01f6b80500a19c32db989c839c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 25 Nov 2017 22:38:34 -0800 Subject: [PATCH 053/155] Add comment describing critical section --- Firmware/MotorControl/low_level.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index a8b78ed1..f7ab9b1f 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -1380,12 +1380,13 @@ void motor_thread(void const * argument) { motor->thread_ready = false; } -/** Function that sets the current encoder count to a desired 32-bit value. */ +// Function that sets the current encoder count to a desired 32-bit value. void setEncoderCount(Motor_t* motor, uint32_t count){ + // Disable interrupts to make a critical section to avoid race condition uint32_t prim = __get_PRIMASK(); __disable_irq(); motor->encoder.encoder_state = count; motor->motor_timer->Instance->CNT = count; motor->encoder.pll_pos = (float)count; __set_PRIMASK(prim); -} \ No newline at end of file +} From a792b932342acfb1d44b77539a28072d7ef6b394 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 14:14:53 -0800 Subject: [PATCH 054/155] split .vscode settings between firmware and tools folder --- .vscode/c_cpp_properties.json | 49 --------------------- Firmware/.vscode/c_cpp_properties.json | 49 +++++++++++++++++++++ {.vscode => Firmware/.vscode}/launch.json | 16 ------- {.vscode => Firmware/.vscode}/settings.json | 0 {.vscode => Firmware/.vscode}/tasks.json | 0 tools/.vscode/launch.json | 24 ++++++++++ 6 files changed, 73 insertions(+), 65 deletions(-) delete mode 100644 .vscode/c_cpp_properties.json create mode 100644 Firmware/.vscode/c_cpp_properties.json rename {.vscode => Firmware/.vscode}/launch.json (60%) rename {.vscode => Firmware/.vscode}/settings.json (100%) rename {.vscode => Firmware/.vscode}/tasks.json (100%) create mode 100644 tools/.vscode/launch.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index 0712bb76..00000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,49 +0,0 @@ -{ - "configurations": [ - { - "name": "Win32", - "includePath": [ - "${workspaceRoot}/Firmware", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/include", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", - "${workspaceRoot}/Firmware/Drivers/DRV8301", - "${workspaceRoot}/Firmware/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", - "${workspaceRoot}/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", - "${workspaceRoot}/Firmware/Drivers/STM32F4xx_HAL_Driver/Inc", - "${workspaceRoot}/Firmware/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", - "${workspaceRoot}/Firmware/Drivers/CMSIS/Device/ST/STM32F4xx/Include", - "${workspaceRoot}/Firmware/Drivers/CMSIS/Include", - "${workspaceRoot}/Firmware/Inc", - "${workspaceRoot}/Firmware/MotorControl", - "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include", - "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include" - ], - "defines": [ - "_DEBUG", - "UNICODE" - ], - "intelliSenseMode": "msvc-x64", - "browse": { - "path": [ - "${workspaceRoot}/Firmware", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/include", - "${workspaceRoot}/Firmware/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", - "${workspaceRoot}/Firmware/Drivers/DRV8301", - "${workspaceRoot}/Firmware/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", - "${workspaceRoot}/Firmware/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", - "${workspaceRoot}/Firmware/Drivers/STM32F4xx_HAL_Driver/Inc", - "${workspaceRoot}/Firmware/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", - "${workspaceRoot}/Firmware/Drivers/CMSIS/Device/ST/STM32F4xx/Include", - "${workspaceRoot}/Firmware/Drivers/CMSIS/Include", - "${workspaceRoot}/Firmware/Inc", - "${workspaceRoot}/Firmware/MotorControl" - ], - "limitSymbolsToIncludedHeaders": true, - "databaseFilename": "" - } - } - ], - "version": 3 -} \ No newline at end of file diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json new file mode 100644 index 00000000..2fb3414a --- /dev/null +++ b/Firmware/.vscode/c_cpp_properties.json @@ -0,0 +1,49 @@ +{ + "configurations": [ + { + "name": "Win32", + "includePath": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl", + "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include", + "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include" + ], + "defines": [ + "_DEBUG", + "UNICODE" + ], + "intelliSenseMode": "msvc-x64", + "browse": { + "path": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl" + ], + "limitSymbolsToIncludedHeaders": true, + "databaseFilename": "" + } + } + ], + "version": 3 +} \ No newline at end of file diff --git a/.vscode/launch.json b/Firmware/.vscode/launch.json similarity index 60% rename from .vscode/launch.json rename to Firmware/.vscode/launch.json index babcd14d..d4141bd1 100644 --- a/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -4,22 +4,6 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - { - "name": "Python", - "type": "python", - "request": "launch", - "stopOnEntry": true, - "pythonPath": "${config:python.pythonPath}", - "program": "${file}", - "cwd": "${workspaceRoot}", - "env": {}, - "envFile": "${workspaceRoot}/.env", - "debugOptions": [ - "WaitOnAbnormalExit", - "WaitOnNormalExit", - "RedirectOutput" - ] - }, { "type": "gdb", "request": "attach", diff --git a/.vscode/settings.json b/Firmware/.vscode/settings.json similarity index 100% rename from .vscode/settings.json rename to Firmware/.vscode/settings.json diff --git a/.vscode/tasks.json b/Firmware/.vscode/tasks.json similarity index 100% rename from .vscode/tasks.json rename to Firmware/.vscode/tasks.json diff --git a/tools/.vscode/launch.json b/tools/.vscode/launch.json new file mode 100644 index 00000000..a3b07eff --- /dev/null +++ b/tools/.vscode/launch.json @@ -0,0 +1,24 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "Python", + "type": "python", + "request": "launch", + "stopOnEntry": true, + "pythonPath": "${config:python.pythonPath}", + "program": "${file}", + "cwd": "${workspaceRoot}", + "env": {}, + "envFile": "${workspaceRoot}/.env", + "debugOptions": [ + "WaitOnAbnormalExit", + "WaitOnNormalExit", + "RedirectOutput" + ] + } + ] +} \ No newline at end of file From 1862a4ed71445ce1e60604589b7e58043130720f Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 14:18:28 -0800 Subject: [PATCH 055/155] add VSCode workspace file --- VSCodeWorkspace.code-workspace | 44 ++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 VSCodeWorkspace.code-workspace diff --git a/VSCodeWorkspace.code-workspace b/VSCodeWorkspace.code-workspace new file mode 100644 index 00000000..60d370af --- /dev/null +++ b/VSCodeWorkspace.code-workspace @@ -0,0 +1,44 @@ +{ + "folders": [ + { + "path": "Firmware" + }, + { + "path": "tools" + } + ], + "settings": { + "files.associations": { + "memory": "cpp", + "utility": "cpp", + "deque": "cpp", + "vector": "cpp", + "array": "cpp", + "*.tcc": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "exception": "cpp", + "functional": "cpp", + "initializer_list": "cpp", + "iosfwd": "cpp", + "istream": "cpp", + "limits": "cpp", + "new": "cpp", + "ostream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "typeinfo": "cpp", + "algorithm": "cpp" + } + } +} \ No newline at end of file From 740faef9e384c1ee1dcc4067daaa58cd684f5cd2 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 14:19:59 -0800 Subject: [PATCH 056/155] autoformat low_level.c --- Firmware/MotorControl/low_level.c | 431 ++++++++++++++---------------- 1 file changed, 207 insertions(+), 224 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 987d3425..4b8cc798 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -3,22 +3,22 @@ // Because of broken cmsis_os.h, we need to include arm_math first, // otherwise chip specific defines are ommited #include -#include // Sets up the correct chip specifc defines required by arm_math +#include // Sets up the correct chip specifc defines required by arm_math #define ARM_MATH_CM4 #include #include +#include +#include #include #include -#include -#include -#include -#include #include -#include +#include +#include #include +#include #include /* Private defines -----------------------------------------------------------*/ @@ -34,39 +34,40 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (600*4) +#define ENCODER_CPR (600 * 4) #define POLE_PAIRS 7 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { - { // M0 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration + { + // M0 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] + .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, // .vel_setpoint = 800.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, .thread_ready = false, // .enable_control = true, // .do_calibration = true, // .calibration_ok = false, .motor_timer = &htim1, - .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2}, + .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, .control_deadline = TIM_1_8_PERIOD_CLOCKS, .last_cpu_time = 0, .current_meas = {0.0f, 0.0f}, @@ -82,13 +83,13 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .shunt_conductance = 1.0f / 0.0005f, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, .v_current_control_integral_q = 0.0f, .Ibus = 0.0f, @@ -100,31 +101,28 @@ Motor_t motors[] = { // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { - .encoder_timer = &htim3, - .encoder_cpr = ENCODER_CPR, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] + .encoder_timer = &htim3, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, + .motor_dir = 0, // set by calib_enc_offset + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] }, .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] }, .timing_log_index = 0, .timing_log = {0}, @@ -137,30 +135,30 @@ Motor_t motors[] = { .calib_vel_threshold = 1.0f, }, }, - { // M1 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration + { // M1 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] + .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, .thread_ready = false, // .enable_control = true, // .do_calibration = true, // .calibration_ok = false, .motor_timer = &htim8, - .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2}, - .control_deadline = (3*TIM_1_8_PERIOD_CLOCKS)/2, + .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, + .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, .last_cpu_time = 0, .current_meas = {0.0f, 0.0f}, .DC_calib = {0.0f, 0.0f}, @@ -175,13 +173,13 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .shunt_conductance = 1.0f / 0.0005f, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, .v_current_control_integral_q = 0.0f, .Ibus = 0.0f, @@ -191,31 +189,28 @@ Motor_t motors[] = { }, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { - .encoder_timer = &htim4, - .encoder_cpr = ENCODER_CPR, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] + .encoder_timer = &htim4, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, + .motor_dir = 0, // set by calib_enc_offset + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] }, .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] }, .timing_log_index = 0, .timing_log = {0}, @@ -238,7 +233,7 @@ static const float current_meas_period = CURRENT_MEAS_PERIOD; static const int current_meas_hz = CURRENT_MEAS_HZ; /* Private variables ---------------------------------------------------------*/ -static float brake_resistance = 0.47f; // [ohm] +static float brake_resistance = 0.47f; // [ohm] /* Function implementations --------------------------------------------------*/ @@ -286,7 +281,7 @@ uint16_t check_timing(Motor_t* motor) { timing = TIM_1_8_PERIOD_CLOCKS + delta; } - if(++(motor->timing_log_index) == TIMING_LOG_SIZE){ + if (++(motor->timing_log_index) == TIMING_LOG_SIZE) { motor->timing_log_index = 0; } motor->timing_log[motor->timing_log_index] = timing; @@ -294,7 +289,7 @@ uint16_t check_timing(Motor_t* motor) { return timing; } -void global_fault(int error){ +void global_fault(int error) { // Disable motors NOW! for (int i = 0; i < num_motors; ++i) { __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motors[i].motor_timer); @@ -309,14 +304,13 @@ void global_fault(int error){ } float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) { - int adcval_bal = (int)ADCValue - (1<<11); - float amp_out_volt = (3.3f/(float)(1<<12)) * (float)adcval_bal; + int adcval_bal = (int)ADCValue - (1 << 11); + float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; float shunt_volt = amp_out_volt * motor->phase_current_rev_gain; float current = shunt_volt * motor->shunt_conductance; return current; } - //-------------------------------- // Initalisation //-------------------------------- @@ -341,42 +335,42 @@ void init_motor_control() { // Set up the gate drivers void DRV8301_setup(Motor_t* motor) { - DRV8301_Obj* gate_driver = &motor->gate_driver; - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; + DRV8301_Obj* gate_driver = &motor->gate_driver; + DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - DRV8301_enable(gate_driver); - DRV8301_setupSpi(gate_driver, local_regs); + DRV8301_enable(gate_driver); + DRV8301_setupSpi(gate_driver, local_regs); - // TODO we can use reporting only if we actually wire up the nOCTW pin - local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - // Overcurrent set to approximately 150A at 100degC. This may need tweaking. - local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - // 20V/V on 500uOhm gives a range of +/- 150A - // 40V/V on 500uOhm gives a range of +/- 75A - local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // TODO we can use reporting only if we actually wire up the nOCTW pin + local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; + // Overcurrent set to approximately 150A at 100degC. This may need tweaking. + local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + // 20V/V on 500uOhm gives a range of +/- 150A + // 40V/V on 500uOhm gives a range of +/- 75A + local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; - switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: - motor->phase_current_rev_gain = 1.0f/10.0f; - break; - case DRV8301_ShuntAmpGain_20VpV: - motor->phase_current_rev_gain = 1.0f/20.0f; - break; - case DRV8301_ShuntAmpGain_40VpV: - motor->phase_current_rev_gain = 1.0f/40.0f; - break; - case DRV8301_ShuntAmpGain_80VpV: - motor->phase_current_rev_gain = 1.0f/80.0f; - break; - } + switch (local_regs->Ctrl_Reg_2.GAIN) { + case DRV8301_ShuntAmpGain_10VpV: + motor->phase_current_rev_gain = 1.0f / 10.0f; + break; + case DRV8301_ShuntAmpGain_20VpV: + motor->phase_current_rev_gain = 1.0f / 20.0f; + break; + case DRV8301_ShuntAmpGain_40VpV: + motor->phase_current_rev_gain = 1.0f / 40.0f; + break; + case DRV8301_ShuntAmpGain_80VpV: + motor->phase_current_rev_gain = 1.0f / 80.0f; + break; + } - local_regs->SndCmd = true; - DRV8301_writeData(gate_driver, local_regs); - local_regs->RcvCmd = true; - DRV8301_readData(gate_driver, local_regs); + local_regs->SndCmd = true; + DRV8301_writeData(gate_driver, local_regs); + local_regs->RcvCmd = true; + DRV8301_readData(gate_driver, local_regs); } -void start_adc_pwm(){ +void start_adc_pwm() { // Enable ADC and interrupts __HAL_ADC_ENABLE(&hadc1); __HAL_ADC_ENABLE(&hadc2); @@ -396,7 +390,7 @@ void start_adc_pwm(){ start_pwm(&htim1); start_pwm(&htim8); // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS/2 - 1*128); + sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); // Motor output starts in the disabled state __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); @@ -404,14 +398,14 @@ void start_adc_pwm(){ // Start brake resistor PWM in floating output configuration htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS+1; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); } -void start_pwm(TIM_HandleTypeDef* htim){ +void start_pwm(TIM_HandleTypeDef* htim) { // Init PWM - int half_load = TIM_1_8_PERIOD_CLOCKS/2; + int half_load = TIM_1_8_PERIOD_CLOCKS / 2; htim->Instance->CCR1 = half_load; htim->Instance->CCR2 = half_load; htim->Instance->CCR3 = half_load; @@ -429,8 +423,7 @@ void start_pwm(TIM_HandleTypeDef* htim){ } void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, - uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { - + uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { // Store intial timer configs uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); @@ -475,7 +468,6 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, htim_b->Instance->BDTR |= MOE_store_b; } - //-------------------------------- // IRQ Callbacks //-------------------------------- @@ -485,30 +477,30 @@ void step_cb(uint16_t GPIO_Pin) { GPIO_PinState dir_pin; float dir; switch (GPIO_Pin) { - case GPIO_1_Pin: - //M0 stepped - if (motors[0].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[0].pos_setpoint += dir * motors[0].counts_per_step; - } - break; - case GPIO_3_Pin: - //M1 stepped - if (motors[1].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[1].pos_setpoint += dir * motors[1].counts_per_step; - } - break; - default: - global_fault(ERROR_UNEXPECTED_STEP_SRC); - break; + case GPIO_1_Pin: + //M0 stepped + if (motors[0].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[0].pos_setpoint += dir * motors[0].counts_per_step; + } + break; + case GPIO_3_Pin: + //M1 stepped + if (motors[1].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[1].pos_setpoint += dir * motors[1].counts_per_step; + } + break; + default: + global_fault(ERROR_UNEXPECTED_STEP_SRC); + break; } } void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * 11.0f / (float)(1<<12); + static const float voltage_scale = 3.3f * 11.0f / (float)(1 << 12); // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); vbus_voltage = ADCValue * voltage_scale; @@ -517,11 +509,11 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. // TODO: Document how the phasing is done, link to timing diagram void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - #define calib_tau 0.2f //@TOTO make more easily configurable +#define calib_tau 0.2f //@TOTO make more easily configurable static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; // Ensure ADCs are expected ones to simplify the logic below - if (!(hadc == &hadc2 || hadc == &hadc3)){ + if (!(hadc == &hadc2 || hadc == &hadc3)) { global_fault(ERROR_ADC_FAILED); return; }; @@ -532,7 +524,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // If we are counting down, we just sampled in SVM vector 7, with zero current Motor_t* motor = injected ? &motors[0] : &motors[1]; bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; - + bool current_meas_not_DC_CAL; if (motor == &motors[1] && counting_down) { // We are measuring M1 DC_CAL here @@ -610,19 +602,18 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } } - //-------------------------------- // Measurement and calibration //-------------------------------- // TODO check Ibeta balance to verify good motor connection bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { - static const float kI = 10.0f; //[(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + static const float kI = 10.0f; //[(V/s)/A] + static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s float test_voltage = 0.0f; for (int i = 0; i < num_test_cycles; ++i) { osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); - if (evt.status != osEventSignal){ + if (evt.status != osEventSignal) { motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; return false; } @@ -636,7 +627,7 @@ bool measure_phase_resistance(Motor_t* motor, float test_current, float max_volt // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_PHASE_RESISTANCE_TIMING; return false; } @@ -672,7 +663,7 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_PHASE_INDUCTANCE_TIMING; return false; } @@ -687,7 +678,7 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // However, the discretisation in the current control loop inverts the same discrepancy float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); float L = v_L / dI_by_dt; - + motor->phase_inductance = L; // TODO arbitrary values set for now if (L < 1e-6f || L > 500e-6f) { @@ -702,15 +693,15 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { static const float start_lock_duration = 1.0f; static const int num_steps = 1024; - static const float dt_step = 1.0f/500.0f; + static const float dt_step = 1.0f / 500.0f; static const float scan_range = 4.0f * M_PI; - const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) + const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; int32_t encvaluesum = 0; // go to encoder zero phase for start_lock_duration to get ready to scan - for (int i = 0; i < start_lock_duration*current_meas_hz; ++i) { + for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; @@ -719,13 +710,13 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } // scan forwards for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { - for (int i = 0; i < dt_step*(float)current_meas_hz; ++i) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; @@ -744,13 +735,13 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } // scan backwards for (float ph = scan_range / 2.0f; ph > -scan_range / 2.0f; ph -= step_size) { - for (int i = 0; i < dt_step*(float)current_meas_hz; ++i) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; @@ -761,7 +752,7 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { return true; } -bool motor_calibration(Motor_t* motor){ +bool motor_calibration(Motor_t* motor) { motor->error = ERROR_NO_ERROR; // #warning(hardcoded values for SK3-5065-280kv!) @@ -777,18 +768,18 @@ bool motor_calibration(Motor_t* motor){ if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) return false; } - + // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] + float current_control_bandwidth = 1000.0f; // [rad/s] motor->current_control.p_gain = current_control_bandwidth * motor->phase_inductance; float plant_pole = motor->phase_resistance / motor->phase_inductance; motor->current_control.i_gain = plant_pole * motor->current_control.p_gain; // Calculate encoder pll gains - float encoder_pll_bandwidth = 1000.0f; // [rad/s] + float encoder_pll_bandwidth = 1000.0f; // [rad/s] motor->encoder.pll_kp = 2.0f * encoder_pll_bandwidth; // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)){ + if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)) { motor->error = ERROR_CALIBRATION_TIMING; return false; } @@ -812,8 +803,8 @@ bool motor_calibration(Motor_t* motor){ bool anti_cogging_calibration(Motor_t* motor) { if (motor->anticogging.calib_anticogging && motor->anticogging.cogging_map != NULL) { float pos_err = motor->anticogging.index - motor->encoder.pll_pos; - if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && - fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { + if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && + fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { motor->anticogging.cogging_map[motor->anticogging.index++] = motor->vel_integrator_current; } if (motor->anticogging.index < ENCODER_CPR) { @@ -834,18 +825,17 @@ bool anti_cogging_calibration(Motor_t* motor) { // Test functions //-------------------------------- -__attribute__((unused)) -void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { +__attribute__((unused)) void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { for (;;) { for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_SCAN_MOTOR_TIMING; return; } @@ -854,8 +844,7 @@ void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { } //TODO integrate as mode in main control loop -__attribute__((unused)) -void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { +__attribute__((unused)) void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { for (;;) { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); update_rotor(motor); @@ -863,26 +852,24 @@ void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); float s = arm_sin_f32(phase); - float v_alpha = c*v_d - s*v_q; - float v_beta = c*v_q + s*v_d; + float v_alpha = c * v_d - s * v_q; + float v_beta = c * v_q + s * v_d; queue_voltage_timings(motor, v_alpha, v_beta); // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_FOC_VOLTAGE_TIMING; return; } } } - //-------------------------------- // Main motor control //-------------------------------- void update_rotor(Motor_t* motor) { - switch (motor->rotor_mode) { case ROTOR_MODE_ENCODER: case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: { @@ -911,11 +898,10 @@ void update_rotor(Motor_t* motor) { encoder->pll_pos += current_meas_period * encoder->pll_kp * delta_pos; encoder->pll_vel += current_meas_period * encoder->pll_ki * delta_pos; } - // Drop through to sensorless if also testing - if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - break; + // Drop through to sensorless if also testing + if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) + break; case ROTOR_MODE_SENSORLESS: { - // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf // In particular, equation 8 (and by extension eqn 4 and 6). @@ -930,8 +916,7 @@ void update_rotor(Motor_t* motor) { // Clarke transform float I_alpha_beta[2] = { -motor->current_meas.phB - motor->current_meas.phC, - one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC) - }; + one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC)}; // alpha-beta vector operations float eta[2]; @@ -995,8 +980,8 @@ void update_rotor(Motor_t* motor) { } break; default: - //TODO error handling - break; + //TODO error handling + break; } } @@ -1016,9 +1001,9 @@ bool using_sensorless(Motor_t* motor) { } float get_rotor_phase(Motor_t* motor) { - if (using_encoder(motor)) + if (using_encoder(motor)) return motor->encoder.phase; - else if (using_sensorless(motor)) + else if (using_sensorless(motor)) return motor->sensorless.phase; else //TODO error handling @@ -1026,9 +1011,9 @@ float get_rotor_phase(Motor_t* motor) { } float get_pll_vel(Motor_t* motor) { - if (using_encoder(motor)) + if (using_encoder(motor)) return motor->encoder.pll_vel; - else if (using_sensorless(motor)) + else if (using_sensorless(motor)) return motor->sensorless.pll_vel; else //TODO error handling @@ -1052,7 +1037,6 @@ bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { } bool spin_up_sensorless(Motor_t* motor) { - static const float ramp_up_time = 0.4f; static const float ramp_up_distance = 4 * M_PI; float ramp_step = current_meas_period / ramp_up_time; @@ -1065,7 +1049,7 @@ bool spin_up_sensorless(Motor_t* motor) { for (float x = 0.0f; x < 1.0f; x += ramp_step) { phase = wrap_pm_pi(ramp_up_distance * x); I_mag = motor->sensorless.spin_up_current * x; - if(!spin_up_timestep(motor, phase, I_mag)) + if (!spin_up_timestep(motor, phase, I_mag)) return false; } @@ -1073,7 +1057,7 @@ bool spin_up_sensorless(Motor_t* motor) { while (vel < motor->sensorless.spin_up_target_vel) { vel += motor->sensorless.spin_up_acceleration * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - if(!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) + if (!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) return false; } @@ -1103,7 +1087,7 @@ void update_brake_current(float brake_current) { // To avoid race condition, first reset timings to safe state // ch3 is low side, ch4 is high side htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS+1; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; htim2.Instance->CCR3 = low_off; htim2.Instance->CCR4 = high_on; } @@ -1134,8 +1118,8 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); float s = arm_sin_f32(phase); - float Id = c*Ialpha + s*Ibeta; - float Iq = c*Ibeta - s*Ialpha; + float Id = c * Ialpha + s * Ibeta; + float Iq = c * Ibeta - s * Ialpha; // Current error float Ierr_d = Id_des - Id; @@ -1153,9 +1137,8 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Vector modulation saturation, lock integrator if saturated // TODO make maximum modulation configurable - float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f/sqrtf(mod_d*mod_d + mod_q*mod_q); - if (mod_scalefactor < 1.0f) - { + float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); + if (mod_scalefactor < 1.0f) { mod_d *= mod_scalefactor; mod_q *= mod_scalefactor; // TODO make decayfactor configurable @@ -1174,17 +1157,17 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Above check doesn't work if last motor is executing voltage control // TODO trigger this update in control_motor_loop instead, // and make voltage control a control mode in it. - float Ibus_sum = 0.0f; - for (int i = 0; i < num_motors; ++i) { - Ibus_sum += motors[i].current_control.Ibus; - } - // Note: function will clip negative values to 0.0f - update_brake_current(-Ibus_sum); + float Ibus_sum = 0.0f; + for (int i = 0; i < num_motors; ++i) { + Ibus_sum += motors[i].current_control.Ibus; + } + // Note: function will clip negative values to 0.0f + update_brake_current(-Ibus_sum); // } // Inverse park transform - float mod_alpha = c*mod_d - s*mod_q; - float mod_beta = c*mod_q + s*mod_d; + float mod_alpha = c * mod_d - s * mod_q; + float mod_beta = c * mod_q + s * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl->final_v_alpha = mod_to_V * mod_alpha; @@ -1195,7 +1178,7 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_FOC_TIMING; return false; } @@ -1204,12 +1187,12 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { void control_motor_loop(Motor_t* motor) { while (*(motor->axis_legacy.enable_control)) { - if(osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal){ + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; break; } update_rotor(motor); - anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking + anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking // Position control // TODO Decide if we want to use encoder or pll position here @@ -1225,7 +1208,7 @@ void control_motor_loop(Motor_t* motor) { // Velocity limiting float vel_lim = motor->vel_limit; - if (vel_des > vel_lim) vel_des = vel_lim; + if (vel_des > vel_lim) vel_des = vel_lim; if (vel_des < -vel_lim) vel_des = -vel_lim; // Velocity control @@ -1234,12 +1217,12 @@ void control_motor_loop(Motor_t* motor) { // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == ENCODER_CPR - 1) - if(motor->anticogging.use_anticogging){ + if (motor->anticogging.use_anticogging) { Iq += motor->anticogging.cogging_map[mod(motor->encoder.pll_pos, ENCODER_CPR)]; } float v_err = vel_des - get_pll_vel(motor); - if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { Iq += motor->vel_gain * v_err; } @@ -1265,7 +1248,7 @@ void control_motor_loop(Motor_t* motor) { } // Velocity integrator (behaviour dependent on limiting) - if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL ) { + if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL) { // reset integral if not in use motor->vel_integrator_current = 0.0f; } else { @@ -1279,8 +1262,8 @@ void control_motor_loop(Motor_t* motor) { motor->current_control.Iq = Iq; // Execute current command - if(!FOC_current(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_current + if (!FOC_current(motor, 0.0f, Iq)) { + break; // in case of error exit loop, motor->error has been set by FOC_current } } From 62714714dbd67356b09399d0eb779b94c783a731 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 14:52:08 -0800 Subject: [PATCH 057/155] fix some merge errors --- Firmware/.vscode/c_cpp_properties.json | 44 ++++++++++++++++++++++++++ Firmware/.vscode/launch.json | 2 +- Firmware/.vscode/settings.json | 2 +- Firmware/.vscode/tasks.json | 4 +-- 4 files changed, 48 insertions(+), 4 deletions(-) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 2fb3414a..2ebf2d39 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -43,6 +43,50 @@ "limitSymbolsToIncludedHeaders": true, "databaseFilename": "" } + }, + { + "name": "Linux", + "includePath": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl", + "/usr/lib/gcc/arm-none-eabi/4.9.3/include", + "/usr/lib/arm-none-eabi/include" + ], + "defines": [ + "_DEBUG", + "UNICODE" + ], + "intelliSenseMode": "msvc-x64", + "browse": { + "path": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl" + ], + "limitSymbolsToIncludedHeaders": true, + "databaseFilename": "" + } } ], "version": 3 diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index d4141bd1..f6e36562 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -12,7 +12,7 @@ "gdbpath": "arm-none-eabi-gdb", "remote": true, "executable": "./build/ODriveFirmware.elf", - "cwd": "${workspaceRoot}/Firmware", + "cwd": "${workspaceRoot}", "printCalls": false, //"preLaunchTask": "openocd", // This isn't working quite right. "autorun": [ diff --git a/Firmware/.vscode/settings.json b/Firmware/.vscode/settings.json index 1089a45c..8f45c82a 100644 --- a/Firmware/.vscode/settings.json +++ b/Firmware/.vscode/settings.json @@ -1,7 +1,7 @@ { "C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 0 }", "files.exclude": { - "Firmware/build": true + "build": true }, "files.associations": { "memory": "cpp", diff --git a/Firmware/.vscode/tasks.json b/Firmware/.vscode/tasks.json index 3dfa660a..33956da9 100644 --- a/Firmware/.vscode/tasks.json +++ b/Firmware/.vscode/tasks.json @@ -6,7 +6,7 @@ { "taskName": "build", "type": "shell", - "command": "(cd Firmware && make -j4)", + "command": "make -j4", "group": { "kind": "build", "isDefault": true @@ -21,7 +21,7 @@ { "taskName": "flash", "type": "shell", - "command": "(cd Firmware && make flash)", + "command": "make flash", "problemMatcher": [] }, { From e4400d1f267b346c97021968e7283c735fb53c0c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 15:26:51 -0800 Subject: [PATCH 058/155] remove old test_communication.py --- tools/test_communication.py | 155 ------------------------------------ 1 file changed, 155 deletions(-) delete mode 100755 tools/test_communication.py diff --git a/tools/test_communication.py b/tools/test_communication.py deleted file mode 100755 index d6eddae5..00000000 --- a/tools/test_communication.py +++ /dev/null @@ -1,155 +0,0 @@ -#!/usr/bin/env python3 - -import argparse -import prompt_toolkit - -def parse_args(): - parser = argparse.ArgumentParser(description='Talk to a ODrive board over USB or serial.\n') - parser.add_argument("-v", "--verbose", action="store_true", - help="print debug information") - group = parser.add_mutually_exclusive_group() - group.add_argument("-d", "--discover", metavar="CHANNELS", action="store", - help="Automatically discover ODrives. Takes a comma-separated list (without spaces) " - "to indicate which connection types should be considered. Possible values are " - "usb and serial. For example \"--discover usb,serial\" indicates " - "that USB and serial ports should be scanned for ODrives. " - "If none of the below options are specified, --discover usb is assumed.") - group.add_argument("-u", "--usb", metavar="BUS:DEVICE", action="store", - help="Specifies the USB port on which the device is connected. " - "For example \"001:014\" means bus 001, device 014. The numbers can be obtained " - "using `lsusb`.") - group.add_argument("-s", "--serial", metavar="PORT", action="store", - help="Specifies the serial port on which the device is connected. " - "For example \"/dev/ttyUSB0\". Use `ls /dev/tty*` to find your port name.") - parser.set_defaults(discover="usb") - return parser.parse_args() - -if __name__ == '__main__': - # parse args before other imports - args = parse_args() - -import sys -import time -import odrive.core - -def noprint(str): - pass - -def print_usage(): - print("ODrive Control Utility") - print("---------------------------------------------------------------------") - print("USAGE:") - print("\tPOSITION_CONTROL:\n\t\tp MOTOR_NUMBER POSITION VELOCITY CURRENT") - print("\tVELOCITY_CONTROL:\n\t\tv MOTOR_NUMBER VELOCITY CURRENT") - print("\tCURRENT_CONTROL:\n\t\tc MOTOR_NUMBER CURRENT") - # TODO: implement the following features: - #print("\tList parameters:\n\t\tmotor0.[TAB]") - #print("\tShow parameter:\n\t\tmotor0.pos_setpoint") - #print("\tChange parameter:\n\t\tmotor0.pos_setpoint = 0") - # print("\tHALT:\n\t\th") - print("\tQuit Python Script:\n\t\tq") - print("---------------------------------------------------------------------") - -def command_prompt_loop(device, history): - """ - Presents the command prompt indefinitely until something goes wrong - """ - # Load all motors - motors = [] - if "motor0" in dir(device): - motors.append(device.motor0) - if "motor1" in dir(device): - motors.append(device.motor1) - - print("Connected - have {} {}".format(len(motors), "motor" if len(motors) == 1 else "motors")) - - while True: - try: - command = prompt_toolkit.prompt( - "ODrive> ", - history=history).strip() - except EOFError: - command = "exit" - - if len(command) == 0: - continue - elif command.startswith("p "): - args = command[2:].split() - try: - motor = motors[int(args[0])] - pos = float(args[1]) - vel = float(args[2]) - cur = float(args[3]) - except (ValueError, IndexError): - print("invalid command format") - continue - motor.set_pos_setpoint(pos, vel, cur) - elif command.startswith("v "): - args = command[2:].split() - try: - motor = motors[int(args[0])] - vel = float(args[1]) - cur = float(args[2]) - except (ValueError, IndexError): - print("invalid command format") - continue - motor.set_vel_setpoint(vel, cur) - elif command.startswith("c "): - args = command[2:].split() - try: - motor = motors[int(args[0])] - cur = float(args[1]) - except (ValueError, IndexError): - print("invalid command format") - continue - motor.set_current_setpoint(cur) - elif command == "h" or command == '?' or command == 'help': - print_usage() - elif command == "q" or command == 'exit': - sys.exit(0) - else: - print("unknown command \"" + command + "\"") - -def main(args): - if (args.verbose): - printer = print - else: - printer = noprint - - history = prompt_toolkit.history.InMemoryHistory() - - print_usage() - - while True: - # Connect to device - if not args.usb is None: - try: - bus = int(args.usb.split(":")[0]) - address = int(args.usb.split(":")[1]) - except (ValueError, IndexError): - print("the --usb argument must look something like this: \"001:014\"") - sys.exit(1) - try: - device = odrive.core.open_usb(bus, address, printer=printer) - except odrive.protocol.DeviceInitException as ex: - print(str(ex)) - sys.exit(1) - elif not args.serial is None: - device = odrive.core.open_serial(args.serial, printer=printer) - else: - print("Waiting for device...") - consider_usb = 'usb' in args.discover.split(',') - consider_serial = 'serial' in args.discover.split(',') - device = odrive.core.find_any(consider_usb, consider_serial, printer=printer) - autoconnected = True - - try: - command_prompt_loop(device, history) - except odrive.protocol.ChannelBrokenException: - print("ODrive disconnected") - if not autoconnected: - sys.exit(1) - - -if __name__ == "__main__": - main(args) From 0c8828f88ea65dd5db92ce5379d95a1cc1ba6b90 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 15:39:16 -0800 Subject: [PATCH 059/155] add c++ include paths to VSCode --- Firmware/.vscode/c_cpp_properties.json | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 2ebf2d39..feba750c 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -17,6 +17,8 @@ "${workspaceRoot}/Inc", "${workspaceRoot}/MotorControl", "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include", + "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1", + "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1/arm-none-eabi", "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include" ], "defines": [ From d39b58a02e9e9a66fd2ba7c3b04d04b89a90cda4 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 16:08:06 -0800 Subject: [PATCH 060/155] Improve documentation, allow UART on v3.3 and higher only --- Firmware/MotorControl/commands.cpp | 2 ++ Firmware/MotorControl/commands.h | 14 ++++++------ Firmware/README.md | 34 ++++++++++++++++++++---------- 3 files changed, 32 insertions(+), 18 deletions(-) diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 308ad9c0..77febe2c 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -297,7 +297,9 @@ void init_communication(void) { case GPIO_MODE_NONE: break; //do nothing case GPIO_MODE_UART: { +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 SetGPIO12toUART(); +#endif } break; case GPIO_MODE_STEP_DIR: { SetGPIO12toStepDir(); diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 1670371a..f78b6b01 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -11,18 +11,18 @@ #include "crc.hpp" // Select which protocol to run on UART (see README for more details) -//#define UART_PROTOCOL_NATIVE -#define UART_PROTOCOL_LEGACY -//#define UART_PROTOCOL_NONE +// #define UART_PROTOCOL_NATIVE +// #define UART_PROTOCOL_LEGACY +#define UART_PROTOCOL_NONE // Select which protocol to run on USB (see README for more details) #define USB_PROTOCOL_NATIVE -//#define USB_PROTOCOL_NATIVE_STREAM_BASED -//#define USB_PROTOCOL_LEGACY -//#define USB_PROTOCOL_NONE +// #define USB_PROTOCOL_NATIVE_STREAM_BASED +// #define USB_PROTOCOL_LEGACY +// #define USB_PROTOCOL_NONE // Use GPIO 1/2 for step/dir input instead of UART -//#define USE_GPIO_MODE_STEP_DIR +// #define USE_GPIO_MODE_STEP_DIR typedef enum { diff --git a/Firmware/README.md b/Firmware/README.md index 5ca5a831..af5fac44 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -10,7 +10,7 @@ If you are a developer, you are encouraged to use the `devel` branch, as it cont - [Configuring parameters](#configuring-parameters) - [Compiling and downloading firmware](#compiling-and-downloading-firmware) -- [Communicating over USB](#communicating-over-usb) +- [Communicating over USB or UART](#communicating-over-usb-or-uart) - [Generating startup code](#generating-startup-code) - [Setting up Eclipse development environment](#setting-up-eclipse-development-environment) - [Notes for Contributors](#notes-for-contributors) @@ -29,7 +29,8 @@ The first thing to set is your board hardware version, located at the top of [In ### Communication configuration If want to use the example python scripts and connect the ODrive via USB, the defaults are fine for you and you can skip this step. -You can select what interface you want to run on USB and UART. The following options are available: +You can select what interface you want to run on USB and GPIO pins. See [Communicating over USB or UART](#communicating-over-usb-or-uart) for more information. +The following options are available in [MotorControl/commands.h](MotorControl/commands.h): __USB__: - `USB_PROTOCOL_NATIVE`: Use the native protocol (recommended for new applications). @@ -44,6 +45,7 @@ __USB__: - `USB_PROTOCOL_NONE`: Ignore USB communication __GPIO 1,2 pins__: +Note that UART is only supported on ODrive v3.3 and higher. - `UART_PROTOCOL_NATIVE`: Use the native protocol (see notes above). - `UART_PROTOCOL_LEGACY`: Use the human-readable legacy protocol Use this option if you control the ODrive with an Arduino. The ODrive Arduino library is not yet updated to the native protocol. @@ -54,7 +56,7 @@ __GPIO 1,2 pins__: The rest of all the parameters are at the top of the [MotorControl/low_level.c](MotorControl/low_level.c) file. Please note that many parameters occur twice, once for each motor. In it's current state, the motor structs contain both tuning parameters, meant to be set by the developer, and static variables, meant to be modified by the software. Unfortunatly these are mixed together right now, but cleaning this up is a high priority task. -It may be helpful to know that the entry point of each of the motor threads is `void motor_thread` at the bottom of [MotorControl/low_level.c](MotorControl/low_level.c). This is like `main` for each motor, and is probably where you should start reading the code. +It may be helpful to know that the entry point of each of the motor threads is `void axis_thread_entry` at the top of [MotorControl/axis.cpp](MotorControl/axis.cpp). This is like `main` for each motor, and is probably where you should start reading the code. ### Mandatory parameters You must set: @@ -65,7 +67,7 @@ You must set: ### Tuning parameters The most important parameters are the limits: * The current limit: `.current_lim = 75.0f, //[A] // Note: consistent with 40v/v gain`. The default current limit, for safety reasons, is set to 10A. This is quite weak, and good for making sure the drive is stable. Once you have tuned the drive, you can increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains. -* The velocity limit: `.vel_limit = 20000.0f, // [counts/s]`. Does what it says on the tin. +* The velocity limit: `.vel_limit = 20000.0f, // [counts/s]`. The motor will be limited to this speed; again the default value is quite slow. The motion control gains are currently manually tuned: * `.pos_gain = 20.0f, // [(counts/s) / counts]` @@ -126,9 +128,21 @@ After installing all of the above, open a Git Bash shell. Continue at section [B * You need to power the board by only **ONE** of the following: VCC(3.3v), 5V, or the main power connection (the DC bus). The USB port (J1) does not power the board. * Run `make flash` in the root of this repository. +If the flashing worked, you can start sending commands. If you want to do that now, you can go to [Communicating over USB or UART](#communicating-over-usb-or-uart). + ### Debugging the firmware -Run `make gdb`. This will reset and halt at program start. Now you can set breakpoints and run the program. If you know how to use gdb, you are good to go. -If you prefer to debug from eclipse, see [Setting up Eclipse development environment](#setting-up-eclipse-development-environment). +The following options are known to work and supported: +* Command line GDB. Run `make gdb`. This will reset and halt at program start. Now you can set breakpoints and run the program. If you know how to use gdb, you are good to go. +* Eclipse, see [Setting up Eclipse development environment](#setting-up-eclipse-development-environment). +* Visual Studio Code. The solution we have is not the most elegant, and if you know a better way, please do help us. + * Make sure you have the Firmware folder as your active folder + * Flash the board with the newest code (starting debug session doesn't do this) + * Tasks -> Run Task -> openocd + * Debug -> Start Debugging + * The processor will reset and halt. + * Set your breakpoints. Note: you can only set breakpoints when the processor is halted, if you set them during run mode, they won't get applied. + * Run + * When you are done, you must kill the openocd task before you are able to flash the board again: Tasks -> Terminate task -> openocd. ## Communicating over USB or UART @@ -137,7 +151,7 @@ There are two simple python scripts to help you get started with controlling the 1. [Install Python 3](https://www.python.org/downloads/), then install dependencies: ``` -pip install pyusb pyserial prompt_toolkit +pip install pyusb pyserial ``` 3. __Linux__: set up USB permissions ``` @@ -148,17 +162,15 @@ pip install pyusb pyserial prompt_toolkit 4. Power the ODrive board (as per the [Flashing the firmware](#flashing-the-firmware) step) 5. Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC 6. __Windows__: Use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb. - * If 'Odrive V3.x' is not in the list of devices upon opening Zadig check 'List All Devices' from the options menu. Connecting to the Odrive board directly and not over a usb hub may also help. With the Odrive selected in the device list choose 'libusb-win32' from the target driver list and select the large 'install driver' button. + * If 'Odrive V3.x' is not in the list of devices upon opening Zadig, check 'List All Devices' from the options menu. With the Odrive selected in the device list choose 'libusb-win32' from the target driver list and select the large 'install driver' button. 7. Run `./tools/demo.py` or `./tools/explore_odrive.py`. - - `demo.py` is a very simple script which will make motor 0 turn back and forth. Take a look at the code if you want to control the ODrive yourself programatically. + - `demo.py` is a very simple script which will make motor 0 turn back and forth. Use this as an example if you want to control the ODrive yourself programatically. - `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --discover serial`. ### From Arduino - [See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) ### Other platforms - See the [protocol specification](https://github.com/madcowswe/ODrive/blob/devel/Firmware/protocol.md) or the [legacy protocol specification](https://github.com/madcowswe/ODrive/blob/devel/Firmware/legacy-protocol.md). From 5c455a3eafae7218ff1f3faa1eee6526325c35b2 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 16:29:46 -0800 Subject: [PATCH 061/155] add IPython support for explore_odrive.py --- tools/explore_odrive.py | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py index 10533a40..0991ee81 100755 --- a/tools/explore_odrive.py +++ b/tools/explore_odrive.py @@ -70,9 +70,20 @@ print('and "my_odrive.motor0.pos_setpoint = 10000"') print('will send motor0 to 10000') print('') -# Enter interactive python shell with tab complete enabled -import code -import rlcompleter -import readline -readline.parse_and_bind("tab: complete") -code.interact(local=locals(), banner='') +try: + # If this assignment works, we are already in interactive mode. + # so just drop out of script to existing shell + interpreter = sys.ps1 +except AttributeError: + # We are not in interactive mode, so let's fire one up + # Though let's be real, IPython is the way to go + print('If you want to have an improved interactive console with pretty colors,') + print('you can run this script in interactive mode with IPython with this command:') + print('ipython -i explore_odrive.py') + print('') + # Enter interactive python shell with tab complete enabled + import code + import rlcompleter + import readline + readline.parse_and_bind("tab: complete") + code.interact(local=locals(), banner='') From afed3a66e2353d5a8cc5c86baeb96f3dcce4db7a Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 16:48:57 -0800 Subject: [PATCH 062/155] update changelog --- Firmware/CHANGELOG.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index a4504408..da7b613f 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,10 +1,17 @@ -## [Unreleased] +## [0.3] ### Added +* **New binary communication protocol** + * This is a much richer and more efficient binary protocol than the old human-readable protocol. + * The old protocol is still available (but will be depricated eventually). You must manually chose to fall back on this protocol if you wish to still use it. +* Support for C++ +* Demo scripts for getting started with commanding ODrive from python * Protection from user setting current_lim higher than is measurable ### Changed * Shunt resistance values for v3.3 and earlier to include extra resistance of PCB +* Refactoring of control code: + * Lifted top layer of low_level.c into Axis.cpp ## [0.2.2] - 2017-11-17 ### Fixed From ed0a344bedaacf61e935380e881c6cce716ec1fb Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 16:49:36 -0800 Subject: [PATCH 063/155] re-remove file inadvertently reintroduced --- .../Src/prev_board_ver/stm32f4xx_it_V3_2.c | 189 ------------------ 1 file changed, 189 deletions(-) delete mode 100644 Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c diff --git a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c b/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c deleted file mode 100644 index 70b706bb..00000000 --- a/Firmware/Src/prev_board_ver/stm32f4xx_it_V3_2.c +++ /dev/null @@ -1,189 +0,0 @@ -/* External variables --------------------------------------------------------*/ -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern ADC_HandleTypeDef hadc1; -extern ADC_HandleTypeDef hadc2; -extern ADC_HandleTypeDef hadc3; - -/******************************************************************************/ -/* Cortex-M4 Processor Interruption and Exception Handlers */ -/******************************************************************************/ - -/** -* @brief This function handles Non maskable interrupt. -*/ -void NMI_Handler(void) -{ - /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ - - /* USER CODE END NonMaskableInt_IRQn 0 */ - /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ - - /* USER CODE END NonMaskableInt_IRQn 1 */ -} - -/** -* @brief This function handles Hard fault interrupt. -*/ -void HardFault_Handler(void) -{ - /* USER CODE BEGIN HardFault_IRQn 0 */ - - /* USER CODE END HardFault_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN HardFault_IRQn 1 */ - - /* USER CODE END HardFault_IRQn 1 */ -} - -/** -* @brief This function handles Memory management fault. -*/ -void MemManage_Handler(void) -{ - /* USER CODE BEGIN MemoryManagement_IRQn 0 */ - - /* USER CODE END MemoryManagement_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN MemoryManagement_IRQn 1 */ - - /* USER CODE END MemoryManagement_IRQn 1 */ -} - -/** -* @brief This function handles Pre-fetch fault, memory access fault. -*/ -void BusFault_Handler(void) -{ - /* USER CODE BEGIN BusFault_IRQn 0 */ - - /* USER CODE END BusFault_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN BusFault_IRQn 1 */ - - /* USER CODE END BusFault_IRQn 1 */ -} - -/** -* @brief This function handles Undefined instruction or illegal state. -*/ -void UsageFault_Handler(void) -{ - /* USER CODE BEGIN UsageFault_IRQn 0 */ - - /* USER CODE END UsageFault_IRQn 0 */ - while (1) - { - } - /* USER CODE BEGIN UsageFault_IRQn 1 */ - - /* USER CODE END UsageFault_IRQn 1 */ -} - -/** -* @brief This function handles Debug monitor. -*/ -void DebugMon_Handler(void) -{ - /* USER CODE BEGIN DebugMonitor_IRQn 0 */ - - /* USER CODE END DebugMonitor_IRQn 0 */ - /* USER CODE BEGIN DebugMonitor_IRQn 1 */ - - /* USER CODE END DebugMonitor_IRQn 1 */ -} - -/** -* @brief This function handles System tick timer. -*/ -void SysTick_Handler(void) -{ - /* USER CODE BEGIN SysTick_IRQn 0 */ - - /* USER CODE END SysTick_IRQn 0 */ - HAL_IncTick(); - osSystickHandler(); - /* USER CODE BEGIN SysTick_IRQn 1 */ - - /* USER CODE END SysTick_IRQn 1 */ -} - -/******************************************************************************/ -/* STM32F4xx Peripheral Interrupt Handlers */ -/* Add here the Interrupt Handlers for the used peripherals. */ -/* For the available peripheral interrupt handler names, */ -/* please refer to the startup file (startup_stm32f4xx.s). */ -/******************************************************************************/ - -/** -* @brief This function handles EXTI line2 interrupt. -*/ -void EXTI2_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI2_IRQn 0 */ - - /* USER CODE END EXTI2_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2); - /* USER CODE BEGIN EXTI2_IRQn 1 */ - - /* USER CODE END EXTI2_IRQn 1 */ -} - -/** -* @brief This function handles EXTI line4 interrupt. -*/ -void EXTI4_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI4_IRQn 0 */ - - /* USER CODE END EXTI4_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4); - /* USER CODE BEGIN EXTI4_IRQn 1 */ - - /* USER CODE END EXTI4_IRQn 1 */ -} - -/** -* @brief This function handles ADC1, ADC2 and ADC3 global interrupts. -*/ -void ADC_IRQHandler(void) -{ - /* USER CODE BEGIN ADC_IRQn 0 */ - - // The HAL's ADC handling mechanism adds many clock cycles of overhead - // So we bypass it and handle the logic ourselves. - //@TODO add vbus meaasurement on adc1 here - ADC_IRQ_Dispatch(&hadc1, &vbus_sense_adc_cb); - ADC_IRQ_Dispatch(&hadc2, &pwm_trig_adc_cb); - ADC_IRQ_Dispatch(&hadc3, &pwm_trig_adc_cb); - - // Bypass HAL - return; - - /* USER CODE END ADC_IRQn 0 */ - HAL_ADC_IRQHandler(&hadc1); - HAL_ADC_IRQHandler(&hadc2); - HAL_ADC_IRQHandler(&hadc3); - /* USER CODE BEGIN ADC_IRQn 1 */ - - /* USER CODE END ADC_IRQn 1 */ -} - -/** -* @brief This function handles USB On The Go FS global interrupt. -*/ -void OTG_FS_IRQHandler(void) -{ - /* USER CODE BEGIN OTG_FS_IRQn 0 */ - - /* USER CODE END OTG_FS_IRQn 0 */ - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - /* USER CODE BEGIN OTG_FS_IRQn 1 */ - - /* USER CODE END OTG_FS_IRQn 1 */ -} \ No newline at end of file From 19dbbec7d73939c8f3ad8a1a18fe256c71e01915 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 26 Nov 2017 17:20:38 -0800 Subject: [PATCH 064/155] minor change --- Firmware/MotorControl/commands.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index f78b6b01..0d9d7c87 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -10,17 +10,17 @@ #include #include "crc.hpp" -// Select which protocol to run on UART (see README for more details) -// #define UART_PROTOCOL_NATIVE -// #define UART_PROTOCOL_LEGACY -#define UART_PROTOCOL_NONE - // Select which protocol to run on USB (see README for more details) #define USB_PROTOCOL_NATIVE // #define USB_PROTOCOL_NATIVE_STREAM_BASED // #define USB_PROTOCOL_LEGACY // #define USB_PROTOCOL_NONE +// Select which protocol to run on UART (see README for more details) +// #define UART_PROTOCOL_NATIVE +// #define UART_PROTOCOL_LEGACY +#define UART_PROTOCOL_NONE + // Use GPIO 1/2 for step/dir input instead of UART // #define USE_GPIO_MODE_STEP_DIR From 671c90afceb40faaf98037becfc8325ccb33df7e Mon Sep 17 00:00:00 2001 From: Capo01 <503426+Capo01@users.noreply.github.com> Date: Mon, 27 Nov 2017 22:10:29 +1100 Subject: [PATCH 065/155] Update README.md --- Firmware/README.md | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/Firmware/README.md b/Firmware/README.md index af5fac44..443b820c 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -145,14 +145,26 @@ The following options are known to work and supported: * When you are done, you must kill the openocd task before you are able to flash the board again: Tasks -> Terminate task -> openocd. ## Communicating over USB or UART - +Warning: If testing USB or UART communication for the first time it is recommend that your motors are free to spin continuously and are not connected to a drivetrain with limited travel. ### From Linux/Windows/macOS There are two simple python scripts to help you get started with controlling the ODrive using python. -1. [Install Python 3](https://www.python.org/downloads/), then install dependencies: +1. [Install Python 3](https://www.python.org/downloads/), then install dependencies pyusb and pyserial: + * __Linux__ ``` pip install pyusb pyserial ``` + * __Windows__ +From the start menu type 'cmd' and open the command prompt. If you only have python3 installed then enter: +``` +pip install pyusb pyserial +``` +If you have python2 and python3 installed concurrently then you must specifiy the location of pip for python3. For me this was at 'C:\Users\ 'username' \AppData\Local\Programs\Python\Python36-32\Scripts\' and so I instead enter: +``` +C:\Users\'username'\AppData\Local\Programs\Python\Python36-32\Scripts\pip install pyusb pyserial +``` +If you have trouble with this step then refer to [this walkthrough.](https://www.youtube.com/watch?v=jnpC_Ib_lbc) + 3. __Linux__: set up USB permissions ``` echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d[0-9][0-9]", MODE="0666"' | sudo tee /etc/udev/rules.d/50-odrive.rules @@ -163,9 +175,11 @@ pip install pyusb pyserial 5. Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC 6. __Windows__: Use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb. * If 'Odrive V3.x' is not in the list of devices upon opening Zadig, check 'List All Devices' from the options menu. With the Odrive selected in the device list choose 'libusb-win32' from the target driver list and select the large 'install driver' button. -7. Run `./tools/demo.py` or `./tools/explore_odrive.py`. +7. Run `./tools/demo.py` or `./tools/explore_odrive.py`. - `demo.py` is a very simple script which will make motor 0 turn back and forth. Use this as an example if you want to control the ODrive yourself programatically. - `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --discover serial`. + * __Windows Users:__ +If you run either of the python scripts and only see a prompt window appear for a split second before it closes then it is likely that you have not installed pyusb and pyserial for python3 correctly. ### From Arduino [See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) From 2c80efc4b23839b73dc3f0398b8895bdb34004ef Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:12:56 -0500 Subject: [PATCH 066/155] Add VSCode information to the README --- Firmware/README.md | 56 ++++++++++----------------------- Firmware/configuring-eclipse.md | 35 +++++++++++++++++++++ Firmware/configuring-vscode.md | 24 ++++++++++++++ 3 files changed, 75 insertions(+), 40 deletions(-) create mode 100644 Firmware/configuring-eclipse.md create mode 100644 Firmware/configuring-vscode.md diff --git a/Firmware/README.md b/Firmware/README.md index af5fac44..a27253f5 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -87,8 +87,11 @@ An upcoming feature will enable automatic tuning. Until then, here is a rough tu By default both motors are enabled, and the default control mode is position control. If you want a different mode, you can change `.control_mode`. To disable a motor, set `.enable_control` and `.do_calibration` to false. +---- ## Compiling and downloading firmware + + ### Getting a programmer Get a programmer that supports SWD (Serial Wire Debugging) and is ST-link v2 compatible. You can get them really cheap on [eBay](http://www.ebay.co.uk/itm/ST-Link-V2-Emulator-Downloader-Programming-Mini-Unit-STM8-STM32-with-20CM-Line-/391173940927?hash=item5b13c8a6bf:g:3g8AAOSw~OdVf-Tu) or many other places. @@ -117,23 +120,32 @@ Install the following: After installing all of the above, open a Git Bash shell. Continue at section [Building the firmware](#building-the-firmware). +### IDE +ODrive is a Makefile project. It does not require an IDE, but the open-source VSCode is recommended. See [Configuring VSCode](configuring-vscode.md) for information on how to do this. + ### Building the firmware * Make sure you have cloned the repository. -* Navigate your terminal (bash/cygwin) to the ODrive/Firmware dir. -* Run `make` in the root of this repository. +* VSCode: + * Tasks -> Run Build Task +* Terminal: + * Navigate your terminal (bash/cygwin) to the ODrive/Firmware dir. + * Run `make` in the root of this repository. ### Flashing the firmware * **Make sure you have [configured the parameters first](#configuring-parameters)** * Connect `SWD`, `SWC`, and `GND` on connector J2 to the programmer. * You need to power the board by only **ONE** of the following: VCC(3.3v), 5V, or the main power connection (the DC bus). The USB port (J1) does not power the board. -* Run `make flash` in the root of this repository. +* VSCode: + * Tasks -> Run Task -> flash +* Terminal: + * Run `make flash` in the root of this repository. If the flashing worked, you can start sending commands. If you want to do that now, you can go to [Communicating over USB or UART](#communicating-over-usb-or-uart). ### Debugging the firmware The following options are known to work and supported: * Command line GDB. Run `make gdb`. This will reset and halt at program start. Now you can set breakpoints and run the program. If you know how to use gdb, you are good to go. -* Eclipse, see [Setting up Eclipse development environment](#setting-up-eclipse-development-environment). +* Eclipse, see [Setting up Eclipse development environment](configuring-eclipse.md). * Visual Studio Code. The solution we have is not the most elegant, and if you know a better way, please do help us. * Make sure you have the Firmware folder as your active folder * Flash the board with the newest code (starting debug session doesn't do this) @@ -189,42 +201,6 @@ You will likely want the pinout for this process. It is available [here](https:/ * Press `Project -> Generate code` * You may need to let it download some drivers and such. -## Setting up Eclipse development environment - -### Install -* Install [Eclipse IDE for C/C++ Developers](http://www.eclipse.org/downloads/packages/eclipse-ide-cc-developers/neon3) -* Install the [OpenOCD Eclipse plugin](http://gnuarmeclipse.github.io/plugins/install/) - -### Import project -* File -> Import -> C/C++ -> Existing Code as Makefile Project -* Browse for existing code location, find the OdriveFirmware root. -* In the Toolchain options, select `Cross GCC` -* Hit Finish -* Build the project (press ctrl-B) - -![Toolchain options](screenshots/CodeAsMakefile.png "Toolchain options") - -### Load the launch configuration -* File -> Import -> Run/Debug -> Launch Configurations -> Next -* Highlight (don't tick) the OdriveFirmare folder in the left column -* Tick OdriveFirmware.launch in the right column -* Hit Finish - -![Launch Configurations](screenshots/ImportLaunch.png "Launch Configurations") - -### Launch! -* Make sure the programmer is connected to the board as per [Flashing the firmware](#flashing-the-firmware). -* Press the down-arrow of the debug symbol in the toolbar, and hit Debug Configurations - * You can also hit Run -> Debug Configurations -* Highlight the debug configuration you imported, called OdriveFirmware. If you do not see the imported launch configuration rename your project to `ODriveFirmware` or edit the launch configuration to match your project name by unfiltering unavailable projects: - -![Launch Configuration Filters](screenshots/LaunchConfigFilter.png "Launch Configuration Filters") - -* Hit Debug -* Eclipse should flash the board for you and the program should start halted on the first instruction in `Main` -* Set beakpoints, step, hit Resume, etc. -* Make some cool features! ;D - ## Notes for Contributors In general the project uses the [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html), except that the default indendtation is 4 spaces, and that the 80 character limit is not very strictly enforced, merely encouraged. diff --git a/Firmware/configuring-eclipse.md b/Firmware/configuring-eclipse.md new file mode 100644 index 00000000..25da57e2 --- /dev/null +++ b/Firmware/configuring-eclipse.md @@ -0,0 +1,35 @@ +## Setting up Eclipse development environment + +### Install +* Install [Eclipse IDE for C/C++ Developers](http://www.eclipse.org/downloads/packages/eclipse-ide-cc-developers/neon3) +* Install the [OpenOCD Eclipse plugin](http://gnuarmeclipse.github.io/plugins/install/) + +### Import project +* File -> Import -> C/C++ -> Existing Code as Makefile Project +* Browse for existing code location, find the OdriveFirmware root. +* In the Toolchain options, select `Cross GCC` +* Hit Finish +* Build the project (press ctrl-B) + +![Toolchain options](screenshots/CodeAsMakefile.png "Toolchain options") + +### Load the launch configuration +* File -> Import -> Run/Debug -> Launch Configurations -> Next +* Highlight (don't tick) the OdriveFirmare folder in the left column +* Tick OdriveFirmware.launch in the right column +* Hit Finish + +![Launch Configurations](screenshots/ImportLaunch.png "Launch Configurations") + +### Launch! +* Make sure the programmer is connected to the board as per [Flashing the firmware](#flashing-the-firmware). +* Press the down-arrow of the debug symbol in the toolbar, and hit Debug Configurations + * You can also hit Run -> Debug Configurations +* Highlight the debug configuration you imported, called OdriveFirmware. If you do not see the imported launch configuration rename your project to `ODriveFirmware` or edit the launch configuration to match your project name by unfiltering unavailable projects: + +![Launch Configuration Filters](screenshots/LaunchConfigFilter.png "Launch Configuration Filters") + +* Hit Debug +* Eclipse should flash the board for you and the program should start halted on the first instruction in `Main` +* Set beakpoints, step, hit Resume, etc. +* Make some cool features! ;D \ No newline at end of file diff --git a/Firmware/configuring-vscode.md b/Firmware/configuring-vscode.md new file mode 100644 index 00000000..5f09055f --- /dev/null +++ b/Firmware/configuring-vscode.md @@ -0,0 +1,24 @@ +# Configuring VSCode + +VSCode is the recommended IDE for working with the ODrive codebase. It is a light-weight text editor with Git integration and GDB debugging functionality. + +Before doing the VSCode setup, make sure you've installed all of your [prerequisites](README.md#installing-prerequisites) + +--- +## Setup Procedure +1. Clone the ODrive repository +1. [Download VSCode](https://code.visualstudio.com/download) +1. Install extensions. This can be done directly from VSCode (Ctrl+Shift+X) + * Required extensions: + * C/C++ + * Native Debug + * Recommended Extensions: + * vscode-icons + * Code Outline + * Include Autocomplete + * Path Autocomplete + * Auto Comment Blocks +1. Restart VSCode +1. Open the VSCode Workspace file, which is located in the root of the ODrive repository. It is called `VSCodeWorkspace.code-workspace`. The first time you open it, VSCode will install some dependencies. If it fails, you may need to [change your proxy settings](https://code.visualstudio.com/docs/getstarted/settings). + +You should now be ready to compile and test the ODrive project. See [Building the Firmware](README.md#building-the-firmware) \ No newline at end of file From f7dd9392f31ea79234060b85838fea94f48e2487 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:13:21 -0500 Subject: [PATCH 067/155] Formatting --- Firmware/README.md | 3 --- 1 file changed, 3 deletions(-) diff --git a/Firmware/README.md b/Firmware/README.md index a27253f5..95d7369c 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -89,9 +89,6 @@ If you want a different mode, you can change `.control_mode`. To disable a motor ---- ## Compiling and downloading firmware - - - ### Getting a programmer Get a programmer that supports SWD (Serial Wire Debugging) and is ST-link v2 compatible. You can get them really cheap on [eBay](http://www.ebay.co.uk/itm/ST-Link-V2-Emulator-Downloader-Programming-Mini-Unit-STM8-STM32-with-20CM-Line-/391173940927?hash=item5b13c8a6bf:g:3g8AAOSw~OdVf-Tu) or many other places. From cc0569ee4cc3f7a9d07f8372b1718d750d9fc858 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:29:10 -0500 Subject: [PATCH 068/155] Move the IDE instructions to other files --- Firmware/README.md | 41 +++++++++++++--------------------- Firmware/configuring-vscode.md | 29 +++++++++++++++++++++++- 2 files changed, 43 insertions(+), 27 deletions(-) diff --git a/Firmware/README.md b/Firmware/README.md index 95d7369c..e1b60ef9 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -12,7 +12,7 @@ If you are a developer, you are encouraged to use the `devel` branch, as it cont - [Compiling and downloading firmware](#compiling-and-downloading-firmware) - [Communicating over USB or UART](#communicating-over-usb-or-uart) - [Generating startup code](#generating-startup-code) -- [Setting up Eclipse development environment](#setting-up-eclipse-development-environment) +- [Setting up an IDE](#setting-up-an-IDE) - [Notes for Contributors](#notes-for-contributors) @@ -115,46 +115,35 @@ Install the following: * [Make for Windows](http://gnuwin32.sourceforge.net/packages/make.htm). Make is used to script the compilation process. Download and run the complete package setup program. Add the path of the binaries to your PATH environment variable. For me this was at `C:\Program Files (x86)\GnuWin32\bin`. For details on how to set your path envirment in windows see [these instructions.](https://www.java.com/en/download/help/path.xml) * OpenOCD. Follow the instructions at [GNU ARM Eclipse - How to install the OpenOCD binaries](http://gnuarmeclipse.github.io/openocd/install/), including the part about ST-LINK/V2 drivers. Add the path of the binaries to your PATH environment variable. For me this was at `C:\Program Files\GNU ARM Eclipse\OpenOCD\0.10.0-201704182147-dev\bin`. -After installing all of the above, open a Git Bash shell. Continue at section [Building the firmware](#building-the-firmware). +--- +## Setting up an IDE +ODrive is a Makefile project. It does not require an IDE, but the open-source VSCode is recommended. It is also possible to use Eclipse. If you'd like to go that route, please see the respective configuration document: -### IDE -ODrive is a Makefile project. It does not require an IDE, but the open-source VSCode is recommended. See [Configuring VSCode](configuring-vscode.md) for information on how to do this. +* [Configuring VSCode](configuring-vscode.md) +* [Configuring Eclipse](configuring-eclipse.md) + +--- +## No IDE Instructions +After installing all of the above, open a Git Bash shell. Continue at section [Building the firmware](#building-the-firmware). ### Building the firmware * Make sure you have cloned the repository. -* VSCode: - * Tasks -> Run Build Task -* Terminal: - * Navigate your terminal (bash/cygwin) to the ODrive/Firmware dir. - * Run `make` in the root of this repository. +* Navigate your terminal (bash/cygwin) to the ODrive/Firmware dir. +* Run `make` in the root of this repository. ### Flashing the firmware * **Make sure you have [configured the parameters first](#configuring-parameters)** * Connect `SWD`, `SWC`, and `GND` on connector J2 to the programmer. * You need to power the board by only **ONE** of the following: VCC(3.3v), 5V, or the main power connection (the DC bus). The USB port (J1) does not power the board. -* VSCode: - * Tasks -> Run Task -> flash -* Terminal: - * Run `make flash` in the root of this repository. +* Run `make flash` in the root of this repository. If the flashing worked, you can start sending commands. If you want to do that now, you can go to [Communicating over USB or UART](#communicating-over-usb-or-uart). ### Debugging the firmware -The following options are known to work and supported: -* Command line GDB. Run `make gdb`. This will reset and halt at program start. Now you can set breakpoints and run the program. If you know how to use gdb, you are good to go. -* Eclipse, see [Setting up Eclipse development environment](configuring-eclipse.md). -* Visual Studio Code. The solution we have is not the most elegant, and if you know a better way, please do help us. - * Make sure you have the Firmware folder as your active folder - * Flash the board with the newest code (starting debug session doesn't do this) - * Tasks -> Run Task -> openocd - * Debug -> Start Debugging - * The processor will reset and halt. - * Set your breakpoints. Note: you can only set breakpoints when the processor is halted, if you set them during run mode, they won't get applied. - * Run - * When you are done, you must kill the openocd task before you are able to flash the board again: Tasks -> Terminate task -> openocd. +* Run `make gdb`. This will reset and halt at program start. Now you can set breakpoints and run the program. If you know how to use gdb, you are good to go. +--- ## Communicating over USB or UART - ### From Linux/Windows/macOS There are two simple python scripts to help you get started with controlling the ODrive using python. diff --git a/Firmware/configuring-vscode.md b/Firmware/configuring-vscode.md index 5f09055f..5b5abf4a 100644 --- a/Firmware/configuring-vscode.md +++ b/Firmware/configuring-vscode.md @@ -21,4 +21,31 @@ Before doing the VSCode setup, make sure you've installed all of your [prerequis 1. Restart VSCode 1. Open the VSCode Workspace file, which is located in the root of the ODrive repository. It is called `VSCodeWorkspace.code-workspace`. The first time you open it, VSCode will install some dependencies. If it fails, you may need to [change your proxy settings](https://code.visualstudio.com/docs/getstarted/settings). -You should now be ready to compile and test the ODrive project. See [Building the Firmware](README.md#building-the-firmware) \ No newline at end of file +You should now be ready to compile and test the ODrive project. + +## Building the Firmware +* Tasks -> Run Build Task + +A terminal window will open with your native shell. VSCode is configured to run the command `make -j4` in this terminal. + +## Flashing the Firmware +* Tasks -> Run Task -> flash + +A terminal window will open with your native shell. VSCode is configured to run the command `make flash` in this terminal. + +If the flashing worked, you can start sending commands. If you want to do that now, you can go to [Communicating over USB or UART](README.md#communicating-over-usb-or-uart). + +## Debugging +The solution we have is not the most elegant, and if you know a better way, please do help us. + * Make sure you have the Firmware folder as your active folder + * Flash the board with the newest code (starting debug session doesn't do this) + * Tasks -> Run Task -> openocd + * Debug -> Start Debugging + * The processor will reset and halt. + * Set your breakpoints. Note: you can only set breakpoints when the processor is halted, if you set them during run mode, they won't get applied. + * Run + * When you are done, you must kill the openocd task before you are able to flash the board again: Tasks -> Terminate task -> openocd. + +## Cleaning the Build +This sometimes needs to be done if you change branches. +* Open a terminal (View -> Integrated Terminal) and enter `make clean` \ No newline at end of file From 91cee4a4b258e4298c95527853add7d2e6897e0a Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:31:59 -0500 Subject: [PATCH 069/155] Fix case issue --- Firmware/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index e1b60ef9..ac9246b1 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -12,7 +12,7 @@ If you are a developer, you are encouraged to use the `devel` branch, as it cont - [Compiling and downloading firmware](#compiling-and-downloading-firmware) - [Communicating over USB or UART](#communicating-over-usb-or-uart) - [Generating startup code](#generating-startup-code) -- [Setting up an IDE](#setting-up-an-IDE) +- [Setting up an IDE](#setting-up-an-ide) - [Notes for Contributors](#notes-for-contributors) From 2888c83de82c554e10dee5a39e6c54292fc44baf Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:32:11 -0500 Subject: [PATCH 070/155] Fix table of contents order --- Firmware/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index ac9246b1..11d59c6f 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -10,9 +10,10 @@ If you are a developer, you are encouraged to use the `devel` branch, as it cont - [Configuring parameters](#configuring-parameters) - [Compiling and downloading firmware](#compiling-and-downloading-firmware) +- [Setting up an IDE](#setting-up-an-ide) +- [Continuing without an IDE](#no-ide-instructions) - [Communicating over USB or UART](#communicating-over-usb-or-uart) - [Generating startup code](#generating-startup-code) -- [Setting up an IDE](#setting-up-an-ide) - [Notes for Contributors](#notes-for-contributors) From 039e0481d6cd45c6c495f5f5b1ce91021e099bcf Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:32:23 -0500 Subject: [PATCH 071/155] Make document links relative --- Firmware/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index 11d59c6f..82162081 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -170,7 +170,7 @@ pip install pyusb pyserial [See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) ### Other platforms -See the [protocol specification](https://github.com/madcowswe/ODrive/blob/devel/Firmware/protocol.md) or the [legacy protocol specification](https://github.com/madcowswe/ODrive/blob/devel/Firmware/legacy-protocol.md). +See the [protocol specification](protocol.md) or the [legacy protocol specification](legacy-protocol.md). ## Generating startup code From ecbf0281e5715c12208af0b0650cafd55bb8d027 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:33:14 -0500 Subject: [PATCH 072/155] Test relative doc link --- Firmware/configuring-vscode.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/configuring-vscode.md b/Firmware/configuring-vscode.md index 5b5abf4a..c8f48401 100644 --- a/Firmware/configuring-vscode.md +++ b/Firmware/configuring-vscode.md @@ -2,7 +2,7 @@ VSCode is the recommended IDE for working with the ODrive codebase. It is a light-weight text editor with Git integration and GDB debugging functionality. -Before doing the VSCode setup, make sure you've installed all of your [prerequisites](README.md#installing-prerequisites) +Before doing the VSCode setup, make sure you've installed all of your [prerequisites](#installing-prerequisites) --- ## Setup Procedure From fec41cfa08e1b36b162639d825d3b29080963a1f Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:33:44 -0500 Subject: [PATCH 073/155] Revert Test relative doc link --- Firmware/configuring-vscode.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/configuring-vscode.md b/Firmware/configuring-vscode.md index c8f48401..5b5abf4a 100644 --- a/Firmware/configuring-vscode.md +++ b/Firmware/configuring-vscode.md @@ -2,7 +2,7 @@ VSCode is the recommended IDE for working with the ODrive codebase. It is a light-weight text editor with Git integration and GDB debugging functionality. -Before doing the VSCode setup, make sure you've installed all of your [prerequisites](#installing-prerequisites) +Before doing the VSCode setup, make sure you've installed all of your [prerequisites](README.md#installing-prerequisites) --- ## Setup Procedure From c073c6eb4b81d1a91a89990bca709f3f9b0c0792 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:35:59 -0500 Subject: [PATCH 074/155] Tweak formatting --- Firmware/configuring-eclipse.md | 10 +++++----- Firmware/configuring-vscode.md | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Firmware/configuring-eclipse.md b/Firmware/configuring-eclipse.md index 25da57e2..e4576255 100644 --- a/Firmware/configuring-eclipse.md +++ b/Firmware/configuring-eclipse.md @@ -1,10 +1,10 @@ -## Setting up Eclipse development environment +# Setting up Eclipse development environment -### Install +## Install * Install [Eclipse IDE for C/C++ Developers](http://www.eclipse.org/downloads/packages/eclipse-ide-cc-developers/neon3) * Install the [OpenOCD Eclipse plugin](http://gnuarmeclipse.github.io/plugins/install/) -### Import project +## Import project * File -> Import -> C/C++ -> Existing Code as Makefile Project * Browse for existing code location, find the OdriveFirmware root. * In the Toolchain options, select `Cross GCC` @@ -13,7 +13,7 @@ ![Toolchain options](screenshots/CodeAsMakefile.png "Toolchain options") -### Load the launch configuration +## Load the launch configuration * File -> Import -> Run/Debug -> Launch Configurations -> Next * Highlight (don't tick) the OdriveFirmare folder in the left column * Tick OdriveFirmware.launch in the right column @@ -21,7 +21,7 @@ ![Launch Configurations](screenshots/ImportLaunch.png "Launch Configurations") -### Launch! +## Launch! * Make sure the programmer is connected to the board as per [Flashing the firmware](#flashing-the-firmware). * Press the down-arrow of the debug symbol in the toolbar, and hit Debug Configurations * You can also hit Run -> Debug Configurations diff --git a/Firmware/configuring-vscode.md b/Firmware/configuring-vscode.md index 5b5abf4a..f2b54ee4 100644 --- a/Firmware/configuring-vscode.md +++ b/Firmware/configuring-vscode.md @@ -4,10 +4,10 @@ VSCode is the recommended IDE for working with the ODrive codebase. It is a lig Before doing the VSCode setup, make sure you've installed all of your [prerequisites](README.md#installing-prerequisites) ---- ## Setup Procedure 1. Clone the ODrive repository 1. [Download VSCode](https://code.visualstudio.com/download) +1. Open VSCode 1. Install extensions. This can be done directly from VSCode (Ctrl+Shift+X) * Required extensions: * C/C++ From c24929842c14ed67c45a9e074c028f034f048ab6 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:38:01 -0500 Subject: [PATCH 075/155] Minor readme formatting --- Firmware/configuring-vscode.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Firmware/configuring-vscode.md b/Firmware/configuring-vscode.md index f2b54ee4..62368874 100644 --- a/Firmware/configuring-vscode.md +++ b/Firmware/configuring-vscode.md @@ -44,7 +44,8 @@ The solution we have is not the most elegant, and if you know a better way, plea * The processor will reset and halt. * Set your breakpoints. Note: you can only set breakpoints when the processor is halted, if you set them during run mode, they won't get applied. * Run - * When you are done, you must kill the openocd task before you are able to flash the board again: Tasks -> Terminate task -> openocd. + * When you are done, you must kill the openocd task before you are able to flash the board again: + * Tasks -> Terminate task -> openocd ## Cleaning the Build This sometimes needs to be done if you change branches. From 1a8a3f5531b85fbbc6877fb9b8724165bfa68122 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:39:28 -0500 Subject: [PATCH 076/155] Add lines for section clarity --- Firmware/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index 82162081..1e1acc2b 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -172,7 +172,7 @@ pip install pyusb pyserial ### Other platforms See the [protocol specification](protocol.md) or the [legacy protocol specification](legacy-protocol.md). - +--- ## Generating startup code **Note:** You do not need to run this step to program the board. This is only required if you wish to update the auto generated code. @@ -188,6 +188,7 @@ You will likely want the pinout for this process. It is available [here](https:/ * Press `Project -> Generate code` * You may need to let it download some drivers and such. +--- ## Notes for Contributors In general the project uses the [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html), except that the default indendtation is 4 spaces, and that the 80 character limit is not very strictly enforced, merely encouraged. From 2885903e71a1c476f3a45cda1fed7c7a38e6990f Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:47:09 -0500 Subject: [PATCH 077/155] Test nbsp markdown rendering on github --- Firmware/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index 1e1acc2b..193f851b 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -172,7 +172,8 @@ pip install pyusb pyserial ### Other platforms See the [protocol specification](protocol.md) or the [legacy protocol specification](legacy-protocol.md). ---- +  + ## Generating startup code **Note:** You do not need to run this step to program the board. This is only required if you wish to update the auto generated code. From 45ed130a324af2cf08002ba65a1f000085e96bb9 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 Nov 2017 19:48:37 -0500 Subject: [PATCH 078/155] Replace --- with html breaks --- Firmware/README.md | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/Firmware/README.md b/Firmware/README.md index 193f851b..585aae4c 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -88,7 +88,7 @@ An upcoming feature will enable automatic tuning. Until then, here is a rough tu By default both motors are enabled, and the default control mode is position control. If you want a different mode, you can change `.control_mode`. To disable a motor, set `.enable_control` and `.do_calibration` to false. ----- +

## Compiling and downloading firmware ### Getting a programmer Get a programmer that supports SWD (Serial Wire Debugging) and is ST-link v2 compatible. You can get them really cheap on [eBay](http://www.ebay.co.uk/itm/ST-Link-V2-Emulator-Downloader-Programming-Mini-Unit-STM8-STM32-with-20CM-Line-/391173940927?hash=item5b13c8a6bf:g:3g8AAOSw~OdVf-Tu) or many other places. @@ -116,14 +116,14 @@ Install the following: * [Make for Windows](http://gnuwin32.sourceforge.net/packages/make.htm). Make is used to script the compilation process. Download and run the complete package setup program. Add the path of the binaries to your PATH environment variable. For me this was at `C:\Program Files (x86)\GnuWin32\bin`. For details on how to set your path envirment in windows see [these instructions.](https://www.java.com/en/download/help/path.xml) * OpenOCD. Follow the instructions at [GNU ARM Eclipse - How to install the OpenOCD binaries](http://gnuarmeclipse.github.io/openocd/install/), including the part about ST-LINK/V2 drivers. Add the path of the binaries to your PATH environment variable. For me this was at `C:\Program Files\GNU ARM Eclipse\OpenOCD\0.10.0-201704182147-dev\bin`. ---- +

## Setting up an IDE ODrive is a Makefile project. It does not require an IDE, but the open-source VSCode is recommended. It is also possible to use Eclipse. If you'd like to go that route, please see the respective configuration document: * [Configuring VSCode](configuring-vscode.md) * [Configuring Eclipse](configuring-eclipse.md) ---- +

## No IDE Instructions After installing all of the above, open a Git Bash shell. Continue at section [Building the firmware](#building-the-firmware). @@ -143,7 +143,7 @@ If the flashing worked, you can start sending commands. If you want to do that n ### Debugging the firmware * Run `make gdb`. This will reset and halt at program start. Now you can set breakpoints and run the program. If you know how to use gdb, you are good to go. ---- +

## Communicating over USB or UART ### From Linux/Windows/macOS There are two simple python scripts to help you get started with controlling the ODrive using python. @@ -172,8 +172,7 @@ pip install pyusb pyserial ### Other platforms See the [protocol specification](protocol.md) or the [legacy protocol specification](legacy-protocol.md). -  - +

## Generating startup code **Note:** You do not need to run this step to program the board. This is only required if you wish to update the auto generated code. @@ -189,7 +188,7 @@ You will likely want the pinout for this process. It is available [here](https:/ * Press `Project -> Generate code` * You may need to let it download some drivers and such. ---- +

## Notes for Contributors In general the project uses the [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html), except that the default indendtation is 4 spaces, and that the 80 character limit is not very strictly enforced, merely encouraged. From b1d8a25ba1742dd5b042a35d33e8eb3608f40efd Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Fri, 1 Dec 2017 22:06:58 -0500 Subject: [PATCH 079/155] Grammar --- Firmware/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index 585aae4c..810a5082 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -118,7 +118,7 @@ Install the following:

## Setting up an IDE -ODrive is a Makefile project. It does not require an IDE, but the open-source VSCode is recommended. It is also possible to use Eclipse. If you'd like to go that route, please see the respective configuration document: +ODrive is a Makefile project. It does not require an IDE, but the open-source IDE VSCode is recommended. It is also possible to use Eclipse. If you'd like to go that route, please see the respective configuration document: * [Configuring VSCode](configuring-vscode.md) * [Configuring Eclipse](configuring-eclipse.md) From 772bbeef12d754b90c101619630f94044ba250d4 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 7 Dec 2017 10:40:24 -0800 Subject: [PATCH 080/155] Update README.md --- Firmware/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/README.md b/Firmware/README.md index eebd5de8..138049b2 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -48,6 +48,7 @@ You must set: #### Tuning parameters The most important parameters are the limits: * The current limit: `.current_lim = 75.0f, //[A] // Note: consistent with 40v/v gain`. The default current limit, for safety reasons, is set to 10A. This is quite weak, and good for making sure the drive is stable. Once you have tuned the drive, you can increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains. + * Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current. * The velocity limit: `.vel_limit = 20000.0f, // [counts/s]`. Does what it says on the tin. The motion control gains are currently manually tuned: From f301d3cac404dd1f31adf6ebe7c96ae2b48a4f00 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 10 Dec 2017 16:16:35 -0800 Subject: [PATCH 081/155] change HW version to v3.4 --- Firmware/Inc/main.h | 2 +- Firmware/MotorControl/commands.cpp | 8 ++++---- Firmware/MotorControl/low_level.c | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index 911d81fe..ef2b2734 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -53,7 +53,7 @@ /* USER CODE BEGIN Includes */ #define HW_VERSION_MAJOR 3 -#define HW_VERSION_MINOR 3 +#define HW_VERSION_MINOR 4 #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 77febe2c..54031438 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -108,8 +108,8 @@ const Endpoint endpoints[] = { Endpoint::make_property("calibration_current", &motors[0].calibration_current), Endpoint::make_property("phase_inductance", const_cast(&motors[0].phase_inductance)), Endpoint::make_property("phase_resistance", const_cast(&motors[0].phase_resistance)), - Endpoint::make_property("current_meas.phB", const_cast(&motors[0].current_meas.phB)), - Endpoint::make_property("current_meas.phC", const_cast(&motors[0].current_meas.phC)), + Endpoint::make_property("current_meas_phB", const_cast(&motors[0].current_meas.phB)), + Endpoint::make_property("current_meas_phC", const_cast(&motors[0].current_meas.phC)), Endpoint::make_property("DC_calib.phB", &motors[0].DC_calib.phB), Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC), Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance), @@ -161,8 +161,8 @@ const Endpoint endpoints[] = { Endpoint::make_property("calibration_current", &motors[1].calibration_current), Endpoint::make_property("phase_inductance", const_cast(&motors[1].phase_inductance)), Endpoint::make_property("phase_resistance", const_cast(&motors[1].phase_resistance)), - Endpoint::make_property("current_meas.phB", const_cast(&motors[1].current_meas.phB)), - Endpoint::make_property("current_meas.phC", const_cast(&motors[1].current_meas.phC)), + Endpoint::make_property("current_meas_phB", const_cast(&motors[1].current_meas.phB)), + Endpoint::make_property("current_meas_phC", const_cast(&motors[1].current_meas.phC)), Endpoint::make_property("DC_calib.phB", &motors[1].DC_calib.phB), Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC), Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance), diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 06e3e1ad..3f906834 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -636,8 +636,8 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // TODO check Ibeta balance to verify good motor connection bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { - static const float kI = 10.0f; //[(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + static const float kI = 10.0f; // [(V/s)/A] + static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s float test_voltage = 0.0f; for (int i = 0; i < num_test_cycles; ++i) { osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); From b3369bb615e83953a4538a2e0bc83a9d047efd66 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 13 Dec 2017 20:44:17 -0500 Subject: [PATCH 082/155] Report correct ODrive Version, fix #84 --- Firmware/Src/usbd_desc.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Firmware/Src/usbd_desc.c b/Firmware/Src/usbd_desc.c index bd00de9a..d98a872c 100644 --- a/Firmware/Src/usbd_desc.c +++ b/Firmware/Src/usbd_desc.c @@ -73,9 +73,11 @@ */ #define USBD_VID 0x1209 #define USBD_LANGID_STRING 1033 -#define USBD_MANUFACTURER_STRING "ODrive" +#define USBD_MANUFACTURER_STRING "ODrive" #define USBD_PID_FS 0x0D31 -#define USBD_PRODUCT_STRING_FS "ODrive v3.1" +#define USBD_PRODUCT_XSTR(s) USBD_PRODUCT_STR(s) +#define USBD_PRODUCT_STR(s) #s +#define USBD_PRODUCT_STRING_FS ODrive HW_VERSION_MAJOR.HW_VERSION_MINOR #define USBD_SERIALNUMBER_STRING_FS "000000000001" #define USBD_CONFIGURATION_STRING_FS "CDC Config" #define USBD_INTERFACE_STRING_FS "CDC Interface" @@ -253,11 +255,11 @@ uint8_t * USBD_FS_ProductStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *len { if(speed == 0) { - USBD_GetString ((uint8_t *)USBD_PRODUCT_STRING_FS, USBD_StrDesc, length); + USBD_GetString ((uint8_t *)USBD_PRODUCT_XSTR(USBD_PRODUCT_STRING_FS), USBD_StrDesc, length); } else { - USBD_GetString ((uint8_t *)USBD_PRODUCT_STRING_FS, USBD_StrDesc, length); + USBD_GetString ((uint8_t *)USBD_PRODUCT_XSTR(USBD_PRODUCT_STRING_FS), USBD_StrDesc, length); } return USBD_StrDesc; } From 7dd7ce2afc8ee8a4995a29f585cad21b3bd83ffb Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 13 Dec 2017 21:44:30 -0500 Subject: [PATCH 083/155] Implement index pin handling --- Firmware/MotorControl/low_level.c | 9 +++++++++ Firmware/MotorControl/low_level.h | 1 + Firmware/Src/gpio.c | 19 +++++++++++++++++++ Firmware/Src/stm32f4xx_it.c | 16 ++++++++++++++++ 4 files changed, 45 insertions(+) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 06e3e1ad..4a0eb7e0 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -35,6 +35,7 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct #define ENCODER_CPR (600 * 4) +#define ENC_USE_INDEX_PIN false #define POLE_PAIRS 7 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); @@ -113,6 +114,7 @@ Motor_t motors[] = { .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { .encoder_timer = &htim3, + .index_found = !(ENC_USE_INDEX_PIN), .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, @@ -207,6 +209,7 @@ Motor_t motors[] = { .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { .encoder_timer = &htim4, + .index_found = !(ENC_USE_INDEX_PIN), .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, @@ -527,6 +530,12 @@ void step_cb(uint16_t GPIO_Pin) { } } +void enc_index_cb(uint16_t GPIO_Pin, int index){ + setEncoderCount(&motors[index], 0); + motors[index].IndexFound = true; +} + + void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { static const float voltage_scale = 3.3f * 11.0f / (float)(1 << 12); // Only one conversion in sequence, so only rank1 diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 342c19ec..d2a73fac 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -102,6 +102,7 @@ typedef struct { typedef struct { TIM_HandleTypeDef* encoder_timer; + bool index_found; int encoder_cpr; int32_t encoder_offset; int32_t encoder_state; diff --git a/Firmware/Src/gpio.c b/Firmware/Src/gpio.c index e751fb61..6af27891 100644 --- a/Firmware/Src/gpio.c +++ b/Firmware/Src/gpio.c @@ -195,11 +195,30 @@ void SetGPIO12toStepDir() { HAL_NVIC_EnableIRQ(EXTI0_IRQn); } +void SetupENCIndexGPIO(){ + /*Configure GPIO pins : PAPin PAPin */ + GPIO_InitStruct.Pin = GPIO_4_Pin|M0_ENC_Z_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /*Configure GPIO pins : PBPin PBPin */ + GPIO_InitStruct.Pin = GPIO_5_Pin|M1_ENC_Z_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); +} + + //Dispatch processing of external interrupts based on source void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { //Step signals for M0 and M1 if (GPIO_Pin & GPIO_1_Pin || GPIO_Pin & GPIO_3_Pin) { step_cb(GPIO_Pin); + } else if(GPIO_Pin & M0_ENC_Z_Pin){ + enc_index_cb(GPIO_Pin, 0); + } else if(GPIO_Pin & M1_ENC_Z_Pin){ + enc_index_cb(GPIO_Pin, 1); } } diff --git a/Firmware/Src/stm32f4xx_it.c b/Firmware/Src/stm32f4xx_it.c index 8f6d6d42..5e97f1f4 100644 --- a/Firmware/Src/stm32f4xx_it.c +++ b/Firmware/Src/stm32f4xx_it.c @@ -313,6 +313,14 @@ void EXTI2_IRQHandler(void) HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2); } +/** +* @brief This function handles EXTI line4 interrupt. +*/ +void EXTI3_IRQHandler(void) +{ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_3); +} + /** * @brief This function handles EXTI line4 interrupt. */ @@ -321,5 +329,13 @@ void EXTI4_IRQHandler(void) HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4); } +/** +* @brief This function handles EXTI line4 interrupt. +*/ +void EXTI15_IRQHandler(void) +{ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15); +} + /* USER CODE END 1 */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ From 67cac6463de93723b0cb77ab55b1dc615a7433b3 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 13 Dec 2017 21:49:22 -0500 Subject: [PATCH 084/155] Don't reset count if we've already found the index... --- Firmware/MotorControl/low_level.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 4a0eb7e0..34927e8e 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -530,12 +530,14 @@ void step_cb(uint16_t GPIO_Pin) { } } -void enc_index_cb(uint16_t GPIO_Pin, int index){ - setEncoderCount(&motors[index], 0); - motors[index].IndexFound = true; +// Triggered when an encoder passes over the "Index" pin +void enc_index_cb(uint16_t GPIO_Pin, int index) { + if (!motors[index].encoder.index_found) { + setEncoderCount(&motors[index], 0); + motors[index].encoder.index_found = true; + } } - void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { static const float voltage_scale = 3.3f * 11.0f / (float)(1 << 12); // Only one conversion in sequence, so only rank1 From a5d6a07cb96aac2052462b38d1cfaa72ad84a165 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 13 Dec 2017 21:52:29 -0500 Subject: [PATCH 085/155] Add missiong GPIO handler --- Firmware/Src/gpio.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/Src/gpio.c b/Firmware/Src/gpio.c index 6af27891..ad624666 100644 --- a/Firmware/Src/gpio.c +++ b/Firmware/Src/gpio.c @@ -196,6 +196,8 @@ void SetGPIO12toStepDir() { } void SetupENCIndexGPIO(){ + GPIO_InitTypeDef GPIO_InitStruct; + /*Configure GPIO pins : PAPin PAPin */ GPIO_InitStruct.Pin = GPIO_4_Pin|M0_ENC_Z_Pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; From 4c1cba8628025abc7343dd0111d680954af3698c Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 13 Dec 2017 21:53:01 -0500 Subject: [PATCH 086/155] Improve parameter naming --- Firmware/MotorControl/low_level.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 34927e8e..d7c795c8 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -531,10 +531,10 @@ void step_cb(uint16_t GPIO_Pin) { } // Triggered when an encoder passes over the "Index" pin -void enc_index_cb(uint16_t GPIO_Pin, int index) { - if (!motors[index].encoder.index_found) { - setEncoderCount(&motors[index], 0); - motors[index].encoder.index_found = true; +void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { + if (!motors[motor_index].encoder.index_found) { + setEncoderCount(&motors[motor_index], 0); + motors[motor_index].encoder.index_found = true; } } From 6715acb4d4b757b62abd7eb2df5ae2cdc7770942 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 13 Dec 2017 21:53:16 -0500 Subject: [PATCH 087/155] Actually declare enc_index_cb --- Firmware/MotorControl/low_level.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index d2a73fac..8ddfae12 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -193,6 +193,7 @@ void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_for void set_current_setpoint(Motor_t* motor, float current_setpoint); void step_cb(uint16_t GPIO_Pin); +void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index); void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); From 2508364f49f6153ddee2ebdfa9b9aad0be8697de Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 13 Dec 2017 22:47:09 -0500 Subject: [PATCH 088/155] Fix pins, enable interrupts --- Firmware/Inc/gpio.h | 1 + Firmware/MotorControl/low_level.c | 1323 ----------------------------- Firmware/Src/gpio.c | 12 +- Firmware/Src/stm32f4xx_it.c | 8 +- 4 files changed, 15 insertions(+), 1329 deletions(-) diff --git a/Firmware/Inc/gpio.h b/Firmware/Inc/gpio.h index 280128eb..74ce09da 100644 --- a/Firmware/Inc/gpio.h +++ b/Firmware/Inc/gpio.h @@ -72,6 +72,7 @@ void MX_GPIO_Init(void); void SetGPIO12toUART(); void SetGPIO12toStepDir(); +void SetupENCIndexGPIO(); /* USER CODE END Prototypes */ diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index d7c795c8..e69de29b 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -1,1323 +0,0 @@ -/* Includes ------------------------------------------------------------------*/ - -// Because of broken cmsis_os.h, we need to include arm_math first, -// otherwise chip specific defines are ommited -#include -#include // Sets up the correct chip specifc defines required by arm_math -#define ARM_MATH_CM4 -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -/* Private defines -----------------------------------------------------------*/ - -// #define DEBUG_PRINT - -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ -// This value is updated by the DC-bus reading ADC. -// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late -float vbus_voltage = 12.0f; - -// TODO stick parameter into struct -#define ENCODER_CPR (600 * 4) -#define ENC_USE_INDEX_PIN false -#define POLE_PAIRS 7 -const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); - -#if HW_VERSION_MAJOR == 3 -#if HW_VERSION_MINOR <= 3 -#define SHUNT_RESISTANCE (675e-6f) -#else -#define SHUNT_RESISTANCE (500e-6f) -#endif -#endif - -// TODO: Migrate to C++, clearly we are actually doing object oriented code here... -// TODO: For nice encapsulation, consider not having the motor objects public -Motor_t motors[] = { - { - // M0 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration - .counts_per_step = 2.0f, - .error = ERROR_NO_ERROR, - .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] - .vel_setpoint = 0.0f, - // .vel_setpoint = 800.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] - // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance - .motor_thread = 0, - .thread_ready = false, - // .enable_control = true, - // .do_calibration = true, - // .calibration_ok = false, - .motor_timer = &htim1, - .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, - .control_deadline = TIM_1_8_PERIOD_CLOCKS, - .last_cpu_time = 0, - .current_meas = {0.0f, 0.0f}, - .DC_calib = {0.0f, 0.0f}, - .gate_driver = { - .spiHandle = &hspi3, - // Note: this board has the EN_Gate pin shared! - .EngpioHandle = EN_GATE_GPIO_Port, - .EngpioNumber = EN_GATE_Pin, - .nCSgpioHandle = M0_nCS_GPIO_Port, - .nCSgpioNumber = M0_nCS_Pin, - .RxTimeOut = false, - .enableTimeOut = false, - }, - // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup - .current_control = { - // Read out max_allowed_current to see max supported value for current_lim. - // You can change DRV8301_ShuntAmpGain to get a different range. - // .current_lim = 75.0f, //[A] - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement - .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f, - .Ibus = 0.0f, - .final_v_alpha = 0.0f, - .final_v_beta = 0.0f, - .Iq = 0.0f, - .max_allowed_current = 0.0f, - }, - // .rotor_mode = ROTOR_MODE_SENSORLESS, - // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, - .rotor_mode = ROTOR_MODE_ENCODER, - .encoder = { - .encoder_timer = &htim3, - .index_found = !(ENC_USE_INDEX_PIN), - .encoder_cpr = ENCODER_CPR, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - }, - .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } - .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] - }, - .timing_log_index = 0, - .timing_log = {0}, - .anticogging = { - .index = 0, - .cogging_map = NULL, - .use_anticogging = false, - .calib_anticogging = false, - .calib_pos_threshold = 1.0f, - .calib_vel_threshold = 1.0f, - }, - }, - { // M1 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration - .counts_per_step = 2.0f, - .error = ERROR_NO_ERROR, - .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] - .vel_setpoint = 0.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance - .motor_thread = 0, - .thread_ready = false, - // .enable_control = true, - // .do_calibration = true, - // .calibration_ok = false, - .motor_timer = &htim8, - .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, - .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, - .last_cpu_time = 0, - .current_meas = {0.0f, 0.0f}, - .DC_calib = {0.0f, 0.0f}, - .gate_driver = { - .spiHandle = &hspi3, - // Note: this board has the EN_Gate pin shared! - .EngpioHandle = EN_GATE_GPIO_Port, - .EngpioNumber = EN_GATE_Pin, - .nCSgpioHandle = M1_nCS_GPIO_Port, - .nCSgpioNumber = M1_nCS_Pin, - .RxTimeOut = false, - .enableTimeOut = false, - }, - // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup - .current_control = { - // Read out max_allowed_current to see max supported value for current_lim. - // You can change DRV8301_ShuntAmpGain to get a different range. - // .current_lim = 75.0f, //[A] - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement - .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f, - .Ibus = 0.0f, - .final_v_alpha = 0.0f, - .final_v_beta = 0.0f, - .Iq = 0.0f, - .max_allowed_current = 0.0f, - }, - .rotor_mode = ROTOR_MODE_ENCODER, - .encoder = { - .encoder_timer = &htim4, - .index_found = !(ENC_USE_INDEX_PIN), - .encoder_cpr = ENCODER_CPR, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - }, - .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } - .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] - }, - .timing_log_index = 0, - .timing_log = {0}, - .anticogging = { - .index = 0, - .cogging_map = NULL, - .use_anticogging = false, - .calib_anticogging = false, - .calib_pos_threshold = 1.0f, - .calib_vel_threshold = 1.0f, - } - } -}; -const size_t num_motors = sizeof(motors) / sizeof(motors[0]); - -/* Private constant data -----------------------------------------------------*/ -static const float one_by_sqrt3 = 0.57735026919f; -static const float sqrt3_by_2 = 0.86602540378f; -static const float current_meas_period = CURRENT_MEAS_PERIOD; -static const int current_meas_hz = CURRENT_MEAS_HZ; - -/* Private variables ---------------------------------------------------------*/ -static float brake_resistance = 0.47f; // [ohm] - -/* Function implementations --------------------------------------------------*/ - -//-------------------------------- -// Command Handling -//-------------------------------- - -void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) { - motor->pos_setpoint = pos_setpoint; - motor->vel_setpoint = vel_feed_forward; - motor->current_setpoint = current_feed_forward; - motor->control_mode = CTRL_MODE_POSITION_CONTROL; -#ifdef DEBUG_PRINT - printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint); -#endif -} - -void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward) { - motor->vel_setpoint = vel_setpoint; - motor->current_setpoint = current_feed_forward; - motor->control_mode = CTRL_MODE_VELOCITY_CONTROL; -#ifdef DEBUG_PRINT - printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint); -#endif -} - -void set_current_setpoint(Motor_t* motor, float current_setpoint) { - motor->current_setpoint = current_setpoint; - motor->control_mode = CTRL_MODE_CURRENT_CONTROL; -#ifdef DEBUG_PRINT - printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint); -#endif -} - -//-------------------------------- -// Utility -//-------------------------------- - -uint16_t check_timing(Motor_t* motor) { - TIM_HandleTypeDef* htim = motor->motor_timer; - uint16_t timing = htim->Instance->CNT; - bool down = htim->Instance->CR1 & TIM_CR1_DIR; - if (down) { - uint16_t delta = TIM_1_8_PERIOD_CLOCKS - timing; - timing = TIM_1_8_PERIOD_CLOCKS + delta; - } - - if (++(motor->timing_log_index) == TIMING_LOG_SIZE) { - motor->timing_log_index = 0; - } - motor->timing_log[motor->timing_log_index] = timing; - - return timing; -} - -void global_fault(int error) { - // Disable motors NOW! - for (int i = 0; i < num_motors; ++i) { - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motors[i].motor_timer); - } - // Set fault codes, etc. - for (int i = 0; i < num_motors; ++i) { - motors[i].error = error; - *(motors[i].axis_legacy.enable_control) = false; - } - // disable brake resistor - update_brake_current(0.0f); -} - -float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) { - int adcval_bal = (int)ADCValue - (1 << 11); - float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; - float shunt_volt = amp_out_volt * motor->phase_current_rev_gain; - float current = shunt_volt * motor->shunt_conductance; - return current; -} - -//-------------------------------- -// Initalisation -//-------------------------------- - -// Initalises the low level motor control and then starts the motor control threads -void init_motor_control() { - // Init gate drivers - DRV8301_setup(&motors[0]); - DRV8301_setup(&motors[1]); - - // Start PWM and enable adc interrupts/callbacks - start_adc_pwm(); - - // Start Encoders - HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); - HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); - - // Wait for current sense calibration to converge - // TODO make timing a function of calibration filter tau - osDelay(1500); -} - -// Set up the gate drivers -void DRV8301_setup(Motor_t* motor) { - DRV8301_Obj* gate_driver = &motor->gate_driver; - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - - DRV8301_enable(gate_driver); - DRV8301_setupSpi(gate_driver, local_regs); - - // TODO we can use reporting only if we actually wire up the nOCTW pin - local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - // Overcurrent set to approximately 150A at 100degC. This may need tweaking. - local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - // 20V/V on 500uOhm gives a range of +/- 150A - // 40V/V on 500uOhm gives a range of +/- 75A - // 20V/V on 666uOhm gives a range of +/- 110A - // 40V/V on 666uOhm gives a range of +/- 55A - local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; - // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; - - switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: - motor->phase_current_rev_gain = 1.0f / 10.0f; - break; - case DRV8301_ShuntAmpGain_20VpV: - motor->phase_current_rev_gain = 1.0f / 20.0f; - break; - case DRV8301_ShuntAmpGain_40VpV: - motor->phase_current_rev_gain = 1.0f / 40.0f; - break; - case DRV8301_ShuntAmpGain_80VpV: - motor->phase_current_rev_gain = 1.0f / 80.0f; - break; - } - - float margin = 0.90f; - float max_input = margin * 0.3f * motor->shunt_conductance; - float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; - motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); - - local_regs->SndCmd = true; - DRV8301_writeData(gate_driver, local_regs); - local_regs->RcvCmd = true; - DRV8301_readData(gate_driver, local_regs); -} - -void start_adc_pwm() { - // Enable ADC and interrupts - __HAL_ADC_ENABLE(&hadc1); - __HAL_ADC_ENABLE(&hadc2); - __HAL_ADC_ENABLE(&hadc3); - // Warp field stabilize. - osDelay(2); - __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC); - __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC); - - // Ensure that debug halting of the core doesn't leave the motor PWM running - __HAL_DBGMCU_FREEZE_TIM1(); - __HAL_DBGMCU_FREEZE_TIM8(); - - start_pwm(&htim1); - start_pwm(&htim8); - // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); - - // Motor output starts in the disabled state - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8); - - // Start brake resistor PWM in floating output configuration - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); -} - -void start_pwm(TIM_HandleTypeDef* htim) { - // Init PWM - int half_load = TIM_1_8_PERIOD_CLOCKS / 2; - htim->Instance->CCR1 = half_load; - htim->Instance->CCR2 = half_load; - htim->Instance->CCR3 = half_load; - - // This hardware obfustication layer really is getting on my nerves - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1); - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2); - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3); - - htim->Instance->CCR4 = 1; - HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4); -} - -void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, - uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { - // Store intial timer configs - uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); - uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); - uint16_t CR2_store = htim_a->Instance->CR2; - uint16_t SMCR_store = htim_b->Instance->SMCR; - // Turn off output - htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE); - htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE); - // Disable both timer counters - htim_a->Instance->CR1 &= ~TIM_CR1_CEN; - htim_b->Instance->CR1 &= ~TIM_CR1_CEN; - // Set first timer to send TRGO on counter enable - htim_a->Instance->CR2 &= ~TIM_CR2_MMS; - htim_a->Instance->CR2 |= TIM_TRGO_ENABLE; - // Set Trigger Source of second timer to the TRGO of the first timer - htim_b->Instance->SMCR &= ~TIM_SMCR_TS; - htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx; - // Set 2nd timer to start on trigger - htim_b->Instance->SMCR &= ~TIM_SMCR_SMS; - htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER; - // Dir bit is read only in center aligned mode, so we clear the mode for now - uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS; - uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS; - htim_a->Instance->CR1 &= ~TIM_CR1_CMS; - htim_b->Instance->CR1 &= ~TIM_CR1_CMS; - // Set both timers to up-counting state - htim_a->Instance->CR1 &= ~TIM_CR1_DIR; - htim_b->Instance->CR1 &= ~TIM_CR1_DIR; - // Restore center aligned mode - htim_a->Instance->CR1 |= CMS_store_a; - htim_b->Instance->CR1 |= CMS_store_b; - // set counter offset - htim_a->Instance->CNT = count_offset; - htim_b->Instance->CNT = 0; - // Start Timer a - htim_a->Instance->CR1 |= (TIM_CR1_CEN); - // Restore timer configs - htim_a->Instance->CR2 = CR2_store; - htim_b->Instance->SMCR = SMCR_store; - // restore output - htim_a->Instance->BDTR |= MOE_store_a; - htim_b->Instance->BDTR |= MOE_store_b; -} - -//-------------------------------- -// IRQ Callbacks -//-------------------------------- - -// step/direction interface -void step_cb(uint16_t GPIO_Pin) { - GPIO_PinState dir_pin; - float dir; - switch (GPIO_Pin) { - case GPIO_1_Pin: - //M0 stepped - if (motors[0].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[0].pos_setpoint += dir * motors[0].counts_per_step; - } - break; - case GPIO_3_Pin: - //M1 stepped - if (motors[1].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[1].pos_setpoint += dir * motors[1].counts_per_step; - } - break; - default: - global_fault(ERROR_UNEXPECTED_STEP_SRC); - break; - } -} - -// Triggered when an encoder passes over the "Index" pin -void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { - if (!motors[motor_index].encoder.index_found) { - setEncoderCount(&motors[motor_index], 0); - motors[motor_index].encoder.index_found = true; - } -} - -void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * 11.0f / (float)(1 << 12); - // Only one conversion in sequence, so only rank1 - uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); - vbus_voltage = ADCValue * voltage_scale; -} - -// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. -// TODO: Document how the phasing is done, link to timing diagram -void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { -#define calib_tau 0.2f //@TOTO make more easily configurable - static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; - - // Ensure ADCs are expected ones to simplify the logic below - if (!(hadc == &hadc2 || hadc == &hadc3)) { - global_fault(ERROR_ADC_FAILED); - return; - }; - - // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion - // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion - // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current - // If we are counting down, we just sampled in SVM vector 7, with zero current - Motor_t* motor = injected ? &motors[0] : &motors[1]; - bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; - - bool current_meas_not_DC_CAL; - if (motor == &motors[1] && counting_down) { - // We are measuring M1 DC_CAL here - current_meas_not_DC_CAL = false; - // Load next timings for M0 (only once is sufficient) - if (hadc == &hadc2) { - motors[0].motor_timer->Instance->CCR1 = motors[0].next_timings[0]; - motors[0].motor_timer->Instance->CCR2 = motors[0].next_timings[1]; - motors[0].motor_timer->Instance->CCR3 = motors[0].next_timings[2]; - } - // Check the timing of the sequencing - check_timing(motor); - - } else if (motor == &motors[0] && !counting_down) { - // We are measuring M0 current here - current_meas_not_DC_CAL = true; - // Load next timings for M1 (only once is sufficient) - if (hadc == &hadc2) { - motors[1].motor_timer->Instance->CCR1 = motors[1].next_timings[0]; - motors[1].motor_timer->Instance->CCR2 = motors[1].next_timings[1]; - motors[1].motor_timer->Instance->CCR3 = motors[1].next_timings[2]; - } - // Check the timing of the sequencing - check_timing(motor); - - } else if (motor == &motors[1] && !counting_down) { - // We are measuring M1 current here - current_meas_not_DC_CAL = true; - // Check the timing of the sequencing - check_timing(motor); - - } else if (motor == &motors[0] && counting_down) { - // We are measuring M0 DC_CAL here - current_meas_not_DC_CAL = false; - // Check the timing of the sequencing - check_timing(motor); - - } else { - global_fault(ERROR_PWM_SRC_FAIL); - return; - } - - uint32_t ADCValue; - if (injected) { - ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); - } else { - ADCValue = HAL_ADC_GetValue(hadc); - } - float current = phase_current_from_adcval(motor, ADCValue); - - if (current_meas_not_DC_CAL) { - // ADC2 and ADC3 record the phB and phC currents concurrently, - // and their interrupts should arrive on the same clock cycle. - // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3. - // Therefore we store the value from ADC2 and signal the thread that the - // measurement is ready when we receive the ADC3 measurement - - // return or continue - if (hadc == &hadc2) { - motor->current_meas.phB = current - motor->DC_calib.phB; - return; - } else { - motor->current_meas.phC = current - motor->DC_calib.phC; - } - // Trigger motor thread - if (motor->thread_ready) - osSignalSet(motor->motor_thread, M_SIGNAL_PH_CURRENT_MEAS); - } else { - // DC_CAL measurement - if (hadc == &hadc2) { - motor->DC_calib.phB += (current - motor->DC_calib.phB) * calib_filter_k; - } else { - motor->DC_calib.phC += (current - motor->DC_calib.phC) * calib_filter_k; - } - } -} - -//-------------------------------- -// Measurement and calibration -//-------------------------------- - -// TODO check Ibeta balance to verify good motor connection -bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { - static const float kI = 10.0f; //[(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s - float test_voltage = 0.0f; - for (int i = 0; i < num_test_cycles; ++i) { - osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); - if (evt.status != osEventSignal) { - motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; - return false; - } - float Ialpha = -(motor->current_meas.phB + motor->current_meas.phC); - test_voltage += (kI * current_meas_period) * (test_current - Ialpha); - if (test_voltage > max_voltage) test_voltage = max_voltage; - if (test_voltage < -max_voltage) test_voltage = -max_voltage; - - // Test voltage along phase A - queue_voltage_timings(motor, test_voltage, 0.0f); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_PHASE_RESISTANCE_TIMING; - return false; - } - } - - // De-energize motor - queue_voltage_timings(motor, 0.0f, 0.0f); - - float R = test_voltage / test_current; - motor->phase_resistance = R; - if (fabs(test_voltage) == fabs(max_voltage) || R < 0.01f || R > 1.0f) { - motor->error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; - return false; - } - return true; -} - -bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high) { - float test_voltages[2] = {voltage_low, voltage_high}; - float Ialphas[2] = {0.0f}; - static const int num_cycles = 5000; - - for (int t = 0; t < num_cycles; ++t) { - for (int i = 0; i < 2; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT; - return false; - } - Ialphas[i] += -motor->current_meas.phB - motor->current_meas.phC; - - // Test voltage along phase A - queue_voltage_timings(motor, test_voltages[i], 0.0f); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_PHASE_INDUCTANCE_TIMING; - return false; - } - } - } - - // De-energize motor - queue_voltage_timings(motor, 0.0f, 0.0f); - - float v_L = 0.5f * (voltage_high - voltage_low); - // Note: A more correct formula would also take into account that there is a finite timestep. - // However, the discretisation in the current control loop inverts the same discrepancy - float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); - float L = v_L / dI_by_dt; - - motor->phase_inductance = L; - // TODO arbitrary values set for now - if (L < 1e-6f || L > 500e-6f) { - motor->error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; - return false; - } - return true; -} - -// TODO: Do the scan with current, not voltage! -// TODO: add check_timing -bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { - static const float start_lock_duration = 1.0f; - static const int num_steps = 1024; - static const float dt_step = 1.0f / 500.0f; - static const float scan_range = 4.0f * M_PI; - const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) - - int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; - int32_t encvaluesum = 0; - - // go to encoder zero phase for start_lock_duration to get ready to scan - for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - queue_voltage_timings(motor, voltage_magnitude, 0.0f); - } - // scan forwards - for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { - for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - } - encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; - } - // check direction - if ((int16_t)motor->encoder.encoder_timer->Instance->CNT > init_enc_val + 8) { - // motor same dir as encoder - motor->encoder.motor_dir = 1; - } else if ((int16_t)motor->encoder.encoder_timer->Instance->CNT < init_enc_val - 8) { - // motor opposite dir as encoder - motor->encoder.motor_dir = -1; - } else { - // Encoder response error - motor->error = ERROR_ENCODER_RESPONSE; - return false; - } - // scan backwards - for (float ph = scan_range / 2.0f; ph > -scan_range / 2.0f; ph -= step_size) { - for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - } - encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; - } - - int offset = encvaluesum / (num_steps * 2); - motor->encoder.encoder_offset = offset; - return true; -} - -bool motor_calibration(Motor_t* motor) { - motor->error = ERROR_NO_ERROR; - - // #warning(hardcoded values for SK3-5065-280kv!) - // float R = 0.0332548246f; - // float L = 7.97315806e-06f; - - if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) - return false; - if (!measure_phase_inductance(motor, -1.0f, 1.0f)) - return false; - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) - return false; - } - - // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] - motor->current_control.p_gain = current_control_bandwidth * motor->phase_inductance; - float plant_pole = motor->phase_resistance / motor->phase_inductance; - motor->current_control.i_gain = plant_pole * motor->current_control.p_gain; - - // Calculate encoder pll gains - float encoder_pll_bandwidth = 1000.0f; // [rad/s] - motor->encoder.pll_kp = 2.0f * encoder_pll_bandwidth; - // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)) { - motor->error = ERROR_CALIBRATION_TIMING; - return false; - } - // Critically damped - motor->encoder.pll_ki = 0.25f * (motor->encoder.pll_kp * motor->encoder.pll_kp); - - // sensorless pll same as encoder (for now) - motor->sensorless.pll_kp = motor->encoder.pll_kp; - motor->sensorless.pll_ki = motor->encoder.pll_ki; - - return true; -} - -/* - * This anti-cogging implementation iterates through each encoder position, - * waits for zero velocity & position error, - * then samples the current required to maintain that position. - * - * This holding current is added as a feedforward term in the control loop. - */ -bool anti_cogging_calibration(Motor_t* motor) { - if (motor->anticogging.calib_anticogging && motor->anticogging.cogging_map != NULL) { - float pos_err = motor->anticogging.index - motor->encoder.pll_pos; - if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && - fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { - motor->anticogging.cogging_map[motor->anticogging.index++] = motor->vel_integrator_current; - } - if (motor->anticogging.index < ENCODER_CPR) { - set_pos_setpoint(motor, motor->anticogging.index, 0.0f, 0.0f); - return false; - } else { - motor->anticogging.index = 0; - set_pos_setpoint(motor, 0.0f, 0.0f, 0.0f); // Send the motor home - motor->anticogging.use_anticogging = true; // We're good to go, enable anti-cogging - motor->anticogging.calib_anticogging = false; - return true; - } - } - return false; -} - -//-------------------------------- -// Test functions -//-------------------------------- - -__attribute__((unused)) void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { - for (;;) { - for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { - osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_SCAN_MOTOR_TIMING; - return; - } - } - } -} - -//TODO integrate as mode in main control loop -__attribute__((unused)) void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { - for (;;) { - osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); - update_rotor(motor); - - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float v_alpha = c * v_d - s * v_q; - float v_beta = c * v_q + s * v_d; - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_FOC_VOLTAGE_TIMING; - return; - } - } -} - -//-------------------------------- -// Main motor control -//-------------------------------- - -void update_rotor(Motor_t* motor) { - switch (motor->rotor_mode) { - case ROTOR_MODE_ENCODER: - case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: { - //for convenience - Encoder_t* encoder = &motor->encoder; - - // update internal encoder state - int16_t delta_enc = (int16_t)encoder->encoder_timer->Instance->CNT - (int16_t)encoder->encoder_state; - encoder->encoder_state += (int32_t)delta_enc; - - // compute electrical phase - int corrected_enc = encoder->encoder_state % ENCODER_CPR; - corrected_enc -= encoder->encoder_offset; - corrected_enc *= encoder->motor_dir; - float ph = elec_rad_per_enc * (float)corrected_enc; - // ph = fmodf(ph, 2*M_PI); - encoder->phase = wrap_pm_pi(ph); - - // run pll (for now pll is in units of encoder counts) - // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? - // Predict current pos - encoder->pll_pos += current_meas_period * encoder->pll_vel; - // discrete phase detector - float delta_pos = (float)(encoder->encoder_state - (int32_t)floorf(encoder->pll_pos)); - // pll feedback - encoder->pll_pos += current_meas_period * encoder->pll_kp * delta_pos; - encoder->pll_vel += current_meas_period * encoder->pll_ki * delta_pos; - } - // Drop through to sensorless if also testing - if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - break; - case ROTOR_MODE_SENSORLESS: { - // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer - // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf - // In particular, equation 8 (and by extension eqn 4 and 6). - - // The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle - // is the one computed two cycles ago. To get the correct measurement, it was stored twice: - // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. - - //for convenience - Sensorless_t* sensorless = &motor->sensorless; - - // Clarke transform - float I_alpha_beta[2] = { - -motor->current_meas.phB - motor->current_meas.phC, - one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC)}; - - // alpha-beta vector operations - float eta[2]; - for (int i = 0; i <= 1; ++i) { - // y is the total flux-driving voltage (see paper eqn 4) - float y = -motor->phase_resistance * I_alpha_beta[i] + sensorless->V_alpha_beta_memory[i]; - // flux dynamics (prediction) - float x_dot = y; - // integrate prediction to current timestep - sensorless->flux_state[i] += x_dot * current_meas_period; - - // eta is the estimated permanent magnet flux (see paper eqn 6) - eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; - } - - // Non-linear observer (see paper eqn 8): - float pm_flux_sqr = sensorless->pm_flux_linkage * sensorless->pm_flux_linkage; - float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; - float bandwidth_factor = 1.0f / (sensorless->pm_flux_linkage * sensorless->pm_flux_linkage); - float eta_factor = 0.5f * (sensorless->observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); - - static float eta_factor_avg_test = 0.0f; - eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test); - - // alpha-beta vector operations - for (int i = 0; i <= 1; ++i) { - // add observer action to flux estimate dynamics - float x_dot = eta_factor * eta[i]; - // convert action to discrete-time - sensorless->flux_state[i] += x_dot * current_meas_period; - // update new eta - eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; - } - - // Flux state estimation done, store V_alpha_beta for next timestep - sensorless->V_alpha_beta_memory[0] = motor->current_control.final_v_alpha; - sensorless->V_alpha_beta_memory[1] = motor->current_control.final_v_beta; - - // PLL - // predict PLL phase with velocity - sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_vel); - // update PLL phase with observer permanent magnet phase - sensorless->phase = fast_atan2(eta[1], eta[0]); - float delta_phase = wrap_pm_pi(sensorless->phase - sensorless->pll_pos); - sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_kp * delta_phase); - // update PLL velocity - sensorless->pll_vel += current_meas_period * sensorless->pll_ki * delta_phase; - - //TODO TEMP TEST HACK - // static int trigger_ctr = 0; - // if (++trigger_ctr >= 3*current_meas_hz) { - // trigger_ctr = 0; - - // //Change to sensorless units - // motor->vel_gain = 15.0f / 200.0f; - // motor->vel_setpoint = 800.0f * motor->encoder.motor_dir; - - // //Change mode - // motor->rotor_mode = ROTOR_MODE_SENSORLESS; - // } - - } break; - default: - //TODO error handling - break; - } -} - -bool using_encoder(Motor_t* motor) { - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - return true; - else - return false; -} - -bool using_sensorless(Motor_t* motor) { - if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) - return true; - else - return false; -} - -float get_rotor_phase(Motor_t* motor) { - if (using_encoder(motor)) - return motor->encoder.phase; - else if (using_sensorless(motor)) - return motor->sensorless.phase; - else - //TODO error handling - return 0.0f; -} - -float get_pll_vel(Motor_t* motor) { - if (using_encoder(motor)) - return motor->encoder.pll_vel; - else if (using_sensorless(motor)) - return motor->sensorless.pll_vel; - else - //TODO error handling - return 0.0f; -} - -// Function that sets the current encoder count to a desired 32-bit value. -void setEncoderCount(Motor_t* motor, uint32_t count) { - // Disable interrupts to make a critical section to avoid race condition - uint32_t prim = __get_PRIMASK(); - __disable_irq(); - motor->encoder.encoder_state = count; - motor->motor_timer->Instance->CNT = count; - motor->encoder.pll_pos = (float)count; - __set_PRIMASK(prim); -} - -bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { - // wait for new timestep - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_SPIN_UP_TIMEOUT; - return false; - } - // run estimator - update_rotor(motor); - // override the phase during spinup - motor->sensorless.phase = phase; - // run current control (with the phase override) - FOC_current(motor, I_mag, 0.0f); - - return true; -} - -bool spin_up_sensorless(Motor_t* motor) { - static const float ramp_up_time = 0.4f; - static const float ramp_up_distance = 4 * M_PI; - float ramp_step = current_meas_period / ramp_up_time; - - float phase = 0.0f; - float vel = ramp_up_distance / ramp_up_time; - float I_mag = 0.0f; - - // spiral up current - for (float x = 0.0f; x < 1.0f; x += ramp_step) { - phase = wrap_pm_pi(ramp_up_distance * x); - I_mag = motor->sensorless.spin_up_current * x; - if (!spin_up_timestep(motor, phase, I_mag)) - return false; - } - - // accelerate - while (vel < motor->sensorless.spin_up_target_vel) { - vel += motor->sensorless.spin_up_acceleration * current_meas_period; - phase = wrap_pm_pi(phase + vel * current_meas_period); - if (!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) - return false; - } - - // // test keep spinning - // while (true) { - // phase = wrap_pm_pi(phase + vel * current_meas_period); - // if(!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) - // return false; - // } - - return true; - - // TODO: check pll vel (abs ratio, 0.8) -} - -void update_brake_current(float brake_current) { - if (brake_current < 0.0f) brake_current = 0.0f; - float brake_duty = brake_current * brake_resistance / vbus_voltage; - - // Duty limit at 90% to allow bootstrap caps to charge - if (brake_duty > 0.9f) brake_duty = 0.9f; - int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); - int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; - if (low_off < 0) low_off = 0; - - // Safe update of low and high side timings - // To avoid race condition, first reset timings to safe state - // ch3 is low side, ch4 is high side - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - htim2.Instance->CCR3 = low_off; - htim2.Instance->CCR4 = high_on; -} - -void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta) { - float tA, tB, tC; - SVM(mod_alpha, mod_beta, &tA, &tB, &tC); - motor->next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); - motor->next_timings[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); - motor->next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); -} - -void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { - float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); - float mod_alpha = vfactor * v_alpha; - float mod_beta = vfactor * v_beta; - queue_modulation_timings(motor, mod_alpha, mod_beta); -} - -bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { - Current_control_t* ictrl = &motor->current_control; - - // Clarke transform - float Ialpha = -motor->current_meas.phB - motor->current_meas.phC; - float Ibeta = one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC); - - // Park transform - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float Id = c * Ialpha + s * Ibeta; - float Iq = c * Ibeta - s * Ialpha; - - // Current error - float Ierr_d = Id_des - Id; - float Ierr_q = Iq_des - Iq; - - // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) - // Apply PI control - float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; - float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; - - float mod_to_V = (2.0f / 3.0f) * vbus_voltage; - float V_to_mod = 1.0f / mod_to_V; - float mod_d = V_to_mod * Vd; - float mod_q = V_to_mod * Vq; - - // Vector modulation saturation, lock integrator if saturated - // TODO make maximum modulation configurable - float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); - if (mod_scalefactor < 1.0f) { - mod_d *= mod_scalefactor; - mod_q *= mod_scalefactor; - // TODO make decayfactor configurable - ictrl->v_current_control_integral_d *= 0.99f; - ictrl->v_current_control_integral_q *= 0.99f; - } else { - ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); - ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); - } - - // Compute estimated bus current - ictrl->Ibus = mod_d * Id + mod_q * Iq; - - // If this is last motor, update brake resistor duty - // if (motor == &motors[num_motors-1]) { - // Above check doesn't work if last motor is executing voltage control - // TODO trigger this update in control_motor_loop instead, - // and make voltage control a control mode in it. - float Ibus_sum = 0.0f; - for (int i = 0; i < num_motors; ++i) { - Ibus_sum += motors[i].current_control.Ibus; - } - // Note: function will clip negative values to 0.0f - update_brake_current(-Ibus_sum); - // } - - // Inverse park transform - float mod_alpha = c * mod_d - s * mod_q; - float mod_beta = c * mod_q + s * mod_d; - - // Report final applied voltage in stationary frame (for sensorles estimator) - ictrl->final_v_alpha = mod_to_V * mod_alpha; - ictrl->final_v_beta = mod_to_V * mod_beta; - - // Apply SVM - queue_modulation_timings(motor, mod_alpha, mod_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_FOC_TIMING; - return false; - } - return true; -} - -void control_motor_loop(Motor_t* motor) { - while (*(motor->axis_legacy.enable_control)) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; - break; - } - update_rotor(motor); - anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking - - // Position control - // TODO Decide if we want to use encoder or pll position here - float vel_des = motor->vel_setpoint; - if (motor->control_mode >= CTRL_MODE_POSITION_CONTROL) { - if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) { - motor->error = ERROR_POS_CTRL_DURING_SENSORLESS; - break; - } - float pos_err = motor->pos_setpoint - motor->encoder.pll_pos; - vel_des += motor->pos_gain * pos_err; - } - - // Velocity limiting - float vel_lim = motor->vel_limit; - if (vel_des > vel_lim) vel_des = vel_lim; - if (vel_des < -vel_lim) vel_des = -vel_lim; - - // Velocity control - float Iq = motor->current_setpoint; - - // Anti-cogging is enabled after calibration - // We get the current position and apply a current feed-forward - // ensuring that we handle negative encoder positions properly (-1 == ENCODER_CPR - 1) - if (motor->anticogging.use_anticogging) { - Iq += motor->anticogging.cogging_map[mod(motor->encoder.pll_pos, ENCODER_CPR)]; - } - - float v_err = vel_des - get_pll_vel(motor); - if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { - Iq += motor->vel_gain * v_err; - } - - // Velocity integral action before limiting - Iq += motor->vel_integrator_current; - - // Apply motor direction correction - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - Iq *= motor->encoder.motor_dir; - } - - // Current limiting - float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->current_control.max_allowed_current); - bool limited = false; - if (Iq > Ilim) { - limited = true; - Iq = Ilim; - } - if (Iq < -Ilim) { - limited = true; - Iq = -Ilim; - } - - // Velocity integrator (behaviour dependent on limiting) - if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL) { - // reset integral if not in use - motor->vel_integrator_current = 0.0f; - } else { - if (limited) { - // TODO make decayfactor configurable - motor->vel_integrator_current *= 0.99f; - } else { - motor->vel_integrator_current += (motor->vel_integrator_gain * current_meas_period) * v_err; - } - } - - motor->current_control.Iq = Iq; - // Execute current command - if (!FOC_current(motor, 0.0f, Iq)) { - break; // in case of error exit loop, motor->error has been set by FOC_current - } - } - - //We are exiting control, reset Ibus, and update brake current - //TODO update brake current from all motors in 1 func - //TODO reset this motor Ibus, then call from here -} diff --git a/Firmware/Src/gpio.c b/Firmware/Src/gpio.c index ad624666..7983667b 100644 --- a/Firmware/Src/gpio.c +++ b/Firmware/Src/gpio.c @@ -197,18 +197,24 @@ void SetGPIO12toStepDir() { void SetupENCIndexGPIO(){ GPIO_InitTypeDef GPIO_InitStruct; - + /*Configure GPIO pins : PAPin PAPin */ - GPIO_InitStruct.Pin = GPIO_4_Pin|M0_ENC_Z_Pin; + GPIO_InitStruct.Pin = M0_ENC_Z_Pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(EXTI15_10_IRQn); + /*Configure GPIO pins : PBPin PBPin */ - GPIO_InitStruct.Pin = GPIO_5_Pin|M1_ENC_Z_Pin; + GPIO_InitStruct.Pin = M1_ENC_Z_Pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(EXTI3_IRQn); } diff --git a/Firmware/Src/stm32f4xx_it.c b/Firmware/Src/stm32f4xx_it.c index 5e97f1f4..064e1036 100644 --- a/Firmware/Src/stm32f4xx_it.c +++ b/Firmware/Src/stm32f4xx_it.c @@ -314,7 +314,7 @@ void EXTI2_IRQHandler(void) } /** -* @brief This function handles EXTI line4 interrupt. +* @brief This function handles EXTI line3 interrupt. */ void EXTI3_IRQHandler(void) { @@ -330,12 +330,14 @@ void EXTI4_IRQHandler(void) } /** -* @brief This function handles EXTI line4 interrupt. +* @brief This function handles EXTI lines 10-15 interrupt. */ -void EXTI15_IRQHandler(void) +void EXTI15_10_IRQHandler(void) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15); } + + /* USER CODE END 1 */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ From e36cbebd0e551987181b6672328a575301cdf924 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 13 Dec 2017 22:49:35 -0500 Subject: [PATCH 089/155] Errr... restore low_level.c --- Firmware/MotorControl/low_level.c | 1324 +++++++++++++++++++++++++++++ 1 file changed, 1324 insertions(+) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index e69de29b..59c86efe 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -0,0 +1,1324 @@ +/* Includes ------------------------------------------------------------------*/ + +// Because of broken cmsis_os.h, we need to include arm_math first, +// otherwise chip specific defines are ommited +#include +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +/* Private defines -----------------------------------------------------------*/ + +// #define DEBUG_PRINT + +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +// This value is updated by the DC-bus reading ADC. +// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late +float vbus_voltage = 12.0f; + +// TODO stick parameter into struct +#define ENCODER_CPR (600 * 4) +#define ENC_USE_INDEX_PIN true +#define POLE_PAIRS 7 +const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); + +#if HW_VERSION_MAJOR == 3 +#if HW_VERSION_MINOR <= 3 +#define SHUNT_RESISTANCE (675e-6f) +#else +#define SHUNT_RESISTANCE (500e-6f) +#endif +#endif + +// TODO: Migrate to C++, clearly we are actually doing object oriented code here... +// TODO: For nice encapsulation, consider not having the motor objects public +Motor_t motors[] = { + { + // M0 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration + .counts_per_step = 2.0f, + .error = ERROR_NO_ERROR, + .pos_setpoint = 0.0f, + .pos_gain = 20.0f, // [(counts/s) / counts] + .vel_setpoint = 0.0f, + // .vel_setpoint = 800.0f, + .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .motor_thread = 0, + .thread_ready = false, + // .enable_control = true, + // .do_calibration = true, + // .calibration_ok = false, + .motor_timer = &htim1, + .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, + .control_deadline = TIM_1_8_PERIOD_CLOCKS, + .last_cpu_time = 0, + .current_meas = {0.0f, 0.0f}, + .DC_calib = {0.0f, 0.0f}, + .gate_driver = { + .spiHandle = &hspi3, + // Note: this board has the EN_Gate pin shared! + .EngpioHandle = EN_GATE_GPIO_Port, + .EngpioNumber = EN_GATE_Pin, + .nCSgpioHandle = M0_nCS_GPIO_Port, + .nCSgpioNumber = M0_nCS_Pin, + .RxTimeOut = false, + .enableTimeOut = false, + }, + // .gate_driver_regs Init by DRV8301_setup + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .current_control = { + // Read out max_allowed_current to see max supported value for current_lim. + // You can change DRV8301_ShuntAmpGain to get a different range. + // .current_lim = 75.0f, //[A] + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + .v_current_control_integral_d = 0.0f, + .v_current_control_integral_q = 0.0f, + .Ibus = 0.0f, + .final_v_alpha = 0.0f, + .final_v_beta = 0.0f, + .Iq = 0.0f, + .max_allowed_current = 0.0f, + }, + // .rotor_mode = ROTOR_MODE_SENSORLESS, + // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, + .rotor_mode = ROTOR_MODE_ENCODER, + .encoder = { + .encoder_timer = &htim3, + .index_found = !(ENC_USE_INDEX_PIN), + .encoder_cpr = ENCODER_CPR, + .encoder_offset = 0, + .encoder_state = 0, + .motor_dir = 0, // set by calib_enc_offset + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + }, + .sensorless = { + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .estimator_good = false, + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] + }, + .timing_log_index = 0, + .timing_log = {0}, + .anticogging = { + .index = 0, + .cogging_map = NULL, + .use_anticogging = false, + .calib_anticogging = false, + .calib_pos_threshold = 1.0f, + .calib_vel_threshold = 1.0f, + }, + }, + { // M1 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration + .counts_per_step = 2.0f, + .error = ERROR_NO_ERROR, + .pos_setpoint = 0.0f, + .pos_gain = 20.0f, // [(counts/s) / counts] + .vel_setpoint = 0.0f, + .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .motor_thread = 0, + .thread_ready = false, + // .enable_control = true, + // .do_calibration = true, + // .calibration_ok = false, + .motor_timer = &htim8, + .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, + .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, + .last_cpu_time = 0, + .current_meas = {0.0f, 0.0f}, + .DC_calib = {0.0f, 0.0f}, + .gate_driver = { + .spiHandle = &hspi3, + // Note: this board has the EN_Gate pin shared! + .EngpioHandle = EN_GATE_GPIO_Port, + .EngpioNumber = EN_GATE_Pin, + .nCSgpioHandle = M1_nCS_GPIO_Port, + .nCSgpioNumber = M1_nCS_Pin, + .RxTimeOut = false, + .enableTimeOut = false, + }, + // .gate_driver_regs Init by DRV8301_setup + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .current_control = { + // Read out max_allowed_current to see max supported value for current_lim. + // You can change DRV8301_ShuntAmpGain to get a different range. + // .current_lim = 75.0f, //[A] + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + .v_current_control_integral_d = 0.0f, + .v_current_control_integral_q = 0.0f, + .Ibus = 0.0f, + .final_v_alpha = 0.0f, + .final_v_beta = 0.0f, + .Iq = 0.0f, + .max_allowed_current = 0.0f, + }, + .rotor_mode = ROTOR_MODE_ENCODER, + .encoder = { + .encoder_timer = &htim4, + .index_found = !(ENC_USE_INDEX_PIN), + .encoder_cpr = ENCODER_CPR, + .encoder_offset = 0, + .encoder_state = 0, + .motor_dir = 0, // set by calib_enc_offset + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + }, + .sensorless = { + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .estimator_good = false, + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] + }, + .timing_log_index = 0, + .timing_log = {0}, + .anticogging = { + .index = 0, + .cogging_map = NULL, + .use_anticogging = false, + .calib_anticogging = false, + .calib_pos_threshold = 1.0f, + .calib_vel_threshold = 1.0f, + } + } +}; +const size_t num_motors = sizeof(motors) / sizeof(motors[0]); + +/* Private constant data -----------------------------------------------------*/ +static const float one_by_sqrt3 = 0.57735026919f; +static const float sqrt3_by_2 = 0.86602540378f; +static const float current_meas_period = CURRENT_MEAS_PERIOD; +static const int current_meas_hz = CURRENT_MEAS_HZ; + +/* Private variables ---------------------------------------------------------*/ +static float brake_resistance = 0.47f; // [ohm] + +/* Function implementations --------------------------------------------------*/ + +//-------------------------------- +// Command Handling +//-------------------------------- + +void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) { + motor->pos_setpoint = pos_setpoint; + motor->vel_setpoint = vel_feed_forward; + motor->current_setpoint = current_feed_forward; + motor->control_mode = CTRL_MODE_POSITION_CONTROL; +#ifdef DEBUG_PRINT + printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint); +#endif +} + +void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward) { + motor->vel_setpoint = vel_setpoint; + motor->current_setpoint = current_feed_forward; + motor->control_mode = CTRL_MODE_VELOCITY_CONTROL; +#ifdef DEBUG_PRINT + printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint); +#endif +} + +void set_current_setpoint(Motor_t* motor, float current_setpoint) { + motor->current_setpoint = current_setpoint; + motor->control_mode = CTRL_MODE_CURRENT_CONTROL; +#ifdef DEBUG_PRINT + printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint); +#endif +} + +//-------------------------------- +// Utility +//-------------------------------- + +uint16_t check_timing(Motor_t* motor) { + TIM_HandleTypeDef* htim = motor->motor_timer; + uint16_t timing = htim->Instance->CNT; + bool down = htim->Instance->CR1 & TIM_CR1_DIR; + if (down) { + uint16_t delta = TIM_1_8_PERIOD_CLOCKS - timing; + timing = TIM_1_8_PERIOD_CLOCKS + delta; + } + + if (++(motor->timing_log_index) == TIMING_LOG_SIZE) { + motor->timing_log_index = 0; + } + motor->timing_log[motor->timing_log_index] = timing; + + return timing; +} + +void global_fault(int error) { + // Disable motors NOW! + for (int i = 0; i < num_motors; ++i) { + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motors[i].motor_timer); + } + // Set fault codes, etc. + for (int i = 0; i < num_motors; ++i) { + motors[i].error = error; + *(motors[i].axis_legacy.enable_control) = false; + } + // disable brake resistor + update_brake_current(0.0f); +} + +float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) { + int adcval_bal = (int)ADCValue - (1 << 11); + float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; + float shunt_volt = amp_out_volt * motor->phase_current_rev_gain; + float current = shunt_volt * motor->shunt_conductance; + return current; +} + +//-------------------------------- +// Initalisation +//-------------------------------- + +// Initalises the low level motor control and then starts the motor control threads +void init_motor_control() { + // Init gate drivers + DRV8301_setup(&motors[0]); + DRV8301_setup(&motors[1]); + + // Start PWM and enable adc interrupts/callbacks + start_adc_pwm(); + + // Start Encoders + HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); + HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); + SetupENCIndexGPIO(); + + // Wait for current sense calibration to converge + // TODO make timing a function of calibration filter tau + osDelay(1500); +} + +// Set up the gate drivers +void DRV8301_setup(Motor_t* motor) { + DRV8301_Obj* gate_driver = &motor->gate_driver; + DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; + + DRV8301_enable(gate_driver); + DRV8301_setupSpi(gate_driver, local_regs); + + // TODO we can use reporting only if we actually wire up the nOCTW pin + local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; + // Overcurrent set to approximately 150A at 100degC. This may need tweaking. + local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + // 20V/V on 500uOhm gives a range of +/- 150A + // 40V/V on 500uOhm gives a range of +/- 75A + // 20V/V on 666uOhm gives a range of +/- 110A + // 40V/V on 666uOhm gives a range of +/- 55A + local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; + + switch (local_regs->Ctrl_Reg_2.GAIN) { + case DRV8301_ShuntAmpGain_10VpV: + motor->phase_current_rev_gain = 1.0f / 10.0f; + break; + case DRV8301_ShuntAmpGain_20VpV: + motor->phase_current_rev_gain = 1.0f / 20.0f; + break; + case DRV8301_ShuntAmpGain_40VpV: + motor->phase_current_rev_gain = 1.0f / 40.0f; + break; + case DRV8301_ShuntAmpGain_80VpV: + motor->phase_current_rev_gain = 1.0f / 80.0f; + break; + } + + float margin = 0.90f; + float max_input = margin * 0.3f * motor->shunt_conductance; + float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; + motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); + + local_regs->SndCmd = true; + DRV8301_writeData(gate_driver, local_regs); + local_regs->RcvCmd = true; + DRV8301_readData(gate_driver, local_regs); +} + +void start_adc_pwm() { + // Enable ADC and interrupts + __HAL_ADC_ENABLE(&hadc1); + __HAL_ADC_ENABLE(&hadc2); + __HAL_ADC_ENABLE(&hadc3); + // Warp field stabilize. + osDelay(2); + __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC); + __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); + __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC); + __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC); + __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC); + + // Ensure that debug halting of the core doesn't leave the motor PWM running + __HAL_DBGMCU_FREEZE_TIM1(); + __HAL_DBGMCU_FREEZE_TIM8(); + + start_pwm(&htim1); + start_pwm(&htim8); + // TODO: explain why this offset + sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); + + // Motor output starts in the disabled state + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8); + + // Start brake resistor PWM in floating output configuration + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); + HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); +} + +void start_pwm(TIM_HandleTypeDef* htim) { + // Init PWM + int half_load = TIM_1_8_PERIOD_CLOCKS / 2; + htim->Instance->CCR1 = half_load; + htim->Instance->CCR2 = half_load; + htim->Instance->CCR3 = half_load; + + // This hardware obfustication layer really is getting on my nerves + HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1); + HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2); + HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2); + HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3); + HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3); + + htim->Instance->CCR4 = 1; + HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4); +} + +void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, + uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { + // Store intial timer configs + uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); + uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); + uint16_t CR2_store = htim_a->Instance->CR2; + uint16_t SMCR_store = htim_b->Instance->SMCR; + // Turn off output + htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE); + htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE); + // Disable both timer counters + htim_a->Instance->CR1 &= ~TIM_CR1_CEN; + htim_b->Instance->CR1 &= ~TIM_CR1_CEN; + // Set first timer to send TRGO on counter enable + htim_a->Instance->CR2 &= ~TIM_CR2_MMS; + htim_a->Instance->CR2 |= TIM_TRGO_ENABLE; + // Set Trigger Source of second timer to the TRGO of the first timer + htim_b->Instance->SMCR &= ~TIM_SMCR_TS; + htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx; + // Set 2nd timer to start on trigger + htim_b->Instance->SMCR &= ~TIM_SMCR_SMS; + htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER; + // Dir bit is read only in center aligned mode, so we clear the mode for now + uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS; + uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS; + htim_a->Instance->CR1 &= ~TIM_CR1_CMS; + htim_b->Instance->CR1 &= ~TIM_CR1_CMS; + // Set both timers to up-counting state + htim_a->Instance->CR1 &= ~TIM_CR1_DIR; + htim_b->Instance->CR1 &= ~TIM_CR1_DIR; + // Restore center aligned mode + htim_a->Instance->CR1 |= CMS_store_a; + htim_b->Instance->CR1 |= CMS_store_b; + // set counter offset + htim_a->Instance->CNT = count_offset; + htim_b->Instance->CNT = 0; + // Start Timer a + htim_a->Instance->CR1 |= (TIM_CR1_CEN); + // Restore timer configs + htim_a->Instance->CR2 = CR2_store; + htim_b->Instance->SMCR = SMCR_store; + // restore output + htim_a->Instance->BDTR |= MOE_store_a; + htim_b->Instance->BDTR |= MOE_store_b; +} + +//-------------------------------- +// IRQ Callbacks +//-------------------------------- + +// step/direction interface +void step_cb(uint16_t GPIO_Pin) { + GPIO_PinState dir_pin; + float dir; + switch (GPIO_Pin) { + case GPIO_1_Pin: + //M0 stepped + if (motors[0].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[0].pos_setpoint += dir * motors[0].counts_per_step; + } + break; + case GPIO_3_Pin: + //M1 stepped + if (motors[1].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[1].pos_setpoint += dir * motors[1].counts_per_step; + } + break; + default: + global_fault(ERROR_UNEXPECTED_STEP_SRC); + break; + } +} + +// Triggered when an encoder passes over the "Index" pin +void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { + if (!motors[motor_index].encoder.index_found) { + setEncoderCount(&motors[motor_index], 0); + motors[motor_index].encoder.index_found = true; + } +} + +void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { + static const float voltage_scale = 3.3f * 11.0f / (float)(1 << 12); + // Only one conversion in sequence, so only rank1 + uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); + vbus_voltage = ADCValue * voltage_scale; +} + +// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. +// TODO: Document how the phasing is done, link to timing diagram +void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { +#define calib_tau 0.2f //@TOTO make more easily configurable + static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; + + // Ensure ADCs are expected ones to simplify the logic below + if (!(hadc == &hadc2 || hadc == &hadc3)) { + global_fault(ERROR_ADC_FAILED); + return; + }; + + // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion + // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion + // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current + // If we are counting down, we just sampled in SVM vector 7, with zero current + Motor_t* motor = injected ? &motors[0] : &motors[1]; + bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; + + bool current_meas_not_DC_CAL; + if (motor == &motors[1] && counting_down) { + // We are measuring M1 DC_CAL here + current_meas_not_DC_CAL = false; + // Load next timings for M0 (only once is sufficient) + if (hadc == &hadc2) { + motors[0].motor_timer->Instance->CCR1 = motors[0].next_timings[0]; + motors[0].motor_timer->Instance->CCR2 = motors[0].next_timings[1]; + motors[0].motor_timer->Instance->CCR3 = motors[0].next_timings[2]; + } + // Check the timing of the sequencing + check_timing(motor); + + } else if (motor == &motors[0] && !counting_down) { + // We are measuring M0 current here + current_meas_not_DC_CAL = true; + // Load next timings for M1 (only once is sufficient) + if (hadc == &hadc2) { + motors[1].motor_timer->Instance->CCR1 = motors[1].next_timings[0]; + motors[1].motor_timer->Instance->CCR2 = motors[1].next_timings[1]; + motors[1].motor_timer->Instance->CCR3 = motors[1].next_timings[2]; + } + // Check the timing of the sequencing + check_timing(motor); + + } else if (motor == &motors[1] && !counting_down) { + // We are measuring M1 current here + current_meas_not_DC_CAL = true; + // Check the timing of the sequencing + check_timing(motor); + + } else if (motor == &motors[0] && counting_down) { + // We are measuring M0 DC_CAL here + current_meas_not_DC_CAL = false; + // Check the timing of the sequencing + check_timing(motor); + + } else { + global_fault(ERROR_PWM_SRC_FAIL); + return; + } + + uint32_t ADCValue; + if (injected) { + ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); + } else { + ADCValue = HAL_ADC_GetValue(hadc); + } + float current = phase_current_from_adcval(motor, ADCValue); + + if (current_meas_not_DC_CAL) { + // ADC2 and ADC3 record the phB and phC currents concurrently, + // and their interrupts should arrive on the same clock cycle. + // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3. + // Therefore we store the value from ADC2 and signal the thread that the + // measurement is ready when we receive the ADC3 measurement + + // return or continue + if (hadc == &hadc2) { + motor->current_meas.phB = current - motor->DC_calib.phB; + return; + } else { + motor->current_meas.phC = current - motor->DC_calib.phC; + } + // Trigger motor thread + if (motor->thread_ready) + osSignalSet(motor->motor_thread, M_SIGNAL_PH_CURRENT_MEAS); + } else { + // DC_CAL measurement + if (hadc == &hadc2) { + motor->DC_calib.phB += (current - motor->DC_calib.phB) * calib_filter_k; + } else { + motor->DC_calib.phC += (current - motor->DC_calib.phC) * calib_filter_k; + } + } +} + +//-------------------------------- +// Measurement and calibration +//-------------------------------- + +// TODO check Ibeta balance to verify good motor connection +bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { + static const float kI = 10.0f; //[(V/s)/A] + static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + float test_voltage = 0.0f; + for (int i = 0; i < num_test_cycles; ++i) { + osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); + if (evt.status != osEventSignal) { + motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; + return false; + } + float Ialpha = -(motor->current_meas.phB + motor->current_meas.phC); + test_voltage += (kI * current_meas_period) * (test_current - Ialpha); + if (test_voltage > max_voltage) test_voltage = max_voltage; + if (test_voltage < -max_voltage) test_voltage = -max_voltage; + + // Test voltage along phase A + queue_voltage_timings(motor, test_voltage, 0.0f); + + // Check we meet deadlines after queueing + motor->last_cpu_time = check_timing(motor); + if (!(motor->last_cpu_time < motor->control_deadline)) { + motor->error = ERROR_PHASE_RESISTANCE_TIMING; + return false; + } + } + + // De-energize motor + queue_voltage_timings(motor, 0.0f, 0.0f); + + float R = test_voltage / test_current; + motor->phase_resistance = R; + if (fabs(test_voltage) == fabs(max_voltage) || R < 0.01f || R > 1.0f) { + motor->error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; + return false; + } + return true; +} + +bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high) { + float test_voltages[2] = {voltage_low, voltage_high}; + float Ialphas[2] = {0.0f}; + static const int num_cycles = 5000; + + for (int t = 0; t < num_cycles; ++t) { + for (int i = 0; i < 2; ++i) { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor->error = ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT; + return false; + } + Ialphas[i] += -motor->current_meas.phB - motor->current_meas.phC; + + // Test voltage along phase A + queue_voltage_timings(motor, test_voltages[i], 0.0f); + + // Check we meet deadlines after queueing + motor->last_cpu_time = check_timing(motor); + if (!(motor->last_cpu_time < motor->control_deadline)) { + motor->error = ERROR_PHASE_INDUCTANCE_TIMING; + return false; + } + } + } + + // De-energize motor + queue_voltage_timings(motor, 0.0f, 0.0f); + + float v_L = 0.5f * (voltage_high - voltage_low); + // Note: A more correct formula would also take into account that there is a finite timestep. + // However, the discretisation in the current control loop inverts the same discrepancy + float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); + float L = v_L / dI_by_dt; + + motor->phase_inductance = L; + // TODO arbitrary values set for now + if (L < 1e-6f || L > 500e-6f) { + motor->error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; + return false; + } + return true; +} + +// TODO: Do the scan with current, not voltage! +// TODO: add check_timing +bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { + static const float start_lock_duration = 1.0f; + static const int num_steps = 1024; + static const float dt_step = 1.0f / 500.0f; + static const float scan_range = 4.0f * M_PI; + const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) + + int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; + int32_t encvaluesum = 0; + + // go to encoder zero phase for start_lock_duration to get ready to scan + for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; + return false; + } + queue_voltage_timings(motor, voltage_magnitude, 0.0f); + } + // scan forwards + for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; + return false; + } + float v_alpha = voltage_magnitude * arm_cos_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); + queue_voltage_timings(motor, v_alpha, v_beta); + } + encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; + } + // check direction + if ((int16_t)motor->encoder.encoder_timer->Instance->CNT > init_enc_val + 8) { + // motor same dir as encoder + motor->encoder.motor_dir = 1; + } else if ((int16_t)motor->encoder.encoder_timer->Instance->CNT < init_enc_val - 8) { + // motor opposite dir as encoder + motor->encoder.motor_dir = -1; + } else { + // Encoder response error + motor->error = ERROR_ENCODER_RESPONSE; + return false; + } + // scan backwards + for (float ph = scan_range / 2.0f; ph > -scan_range / 2.0f; ph -= step_size) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; + return false; + } + float v_alpha = voltage_magnitude * arm_cos_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); + queue_voltage_timings(motor, v_alpha, v_beta); + } + encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; + } + + int offset = encvaluesum / (num_steps * 2); + motor->encoder.encoder_offset = offset; + return true; +} + +bool motor_calibration(Motor_t* motor) { + motor->error = ERROR_NO_ERROR; + + // #warning(hardcoded values for SK3-5065-280kv!) + // float R = 0.0332548246f; + // float L = 7.97315806e-06f; + + if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) + return false; + if (!measure_phase_inductance(motor, -1.0f, 1.0f)) + return false; + if (motor->rotor_mode == ROTOR_MODE_ENCODER || + motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { + if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) + return false; + } + + // Calculate current control gains + float current_control_bandwidth = 1000.0f; // [rad/s] + motor->current_control.p_gain = current_control_bandwidth * motor->phase_inductance; + float plant_pole = motor->phase_resistance / motor->phase_inductance; + motor->current_control.i_gain = plant_pole * motor->current_control.p_gain; + + // Calculate encoder pll gains + float encoder_pll_bandwidth = 1000.0f; // [rad/s] + motor->encoder.pll_kp = 2.0f * encoder_pll_bandwidth; + // Check that we don't get problems with discrete time approximation + if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)) { + motor->error = ERROR_CALIBRATION_TIMING; + return false; + } + // Critically damped + motor->encoder.pll_ki = 0.25f * (motor->encoder.pll_kp * motor->encoder.pll_kp); + + // sensorless pll same as encoder (for now) + motor->sensorless.pll_kp = motor->encoder.pll_kp; + motor->sensorless.pll_ki = motor->encoder.pll_ki; + + return true; +} + +/* + * This anti-cogging implementation iterates through each encoder position, + * waits for zero velocity & position error, + * then samples the current required to maintain that position. + * + * This holding current is added as a feedforward term in the control loop. + */ +bool anti_cogging_calibration(Motor_t* motor) { + if (motor->anticogging.calib_anticogging && motor->anticogging.cogging_map != NULL) { + float pos_err = motor->anticogging.index - motor->encoder.pll_pos; + if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && + fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { + motor->anticogging.cogging_map[motor->anticogging.index++] = motor->vel_integrator_current; + } + if (motor->anticogging.index < ENCODER_CPR) { + set_pos_setpoint(motor, motor->anticogging.index, 0.0f, 0.0f); + return false; + } else { + motor->anticogging.index = 0; + set_pos_setpoint(motor, 0.0f, 0.0f, 0.0f); // Send the motor home + motor->anticogging.use_anticogging = true; // We're good to go, enable anti-cogging + motor->anticogging.calib_anticogging = false; + return true; + } + } + return false; +} + +//-------------------------------- +// Test functions +//-------------------------------- + +__attribute__((unused)) void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { + for (;;) { + for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { + osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); + float v_alpha = voltage_magnitude * arm_cos_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); + queue_voltage_timings(motor, v_alpha, v_beta); + + // Check we meet deadlines after queueing + motor->last_cpu_time = check_timing(motor); + if (!(motor->last_cpu_time < motor->control_deadline)) { + motor->error = ERROR_SCAN_MOTOR_TIMING; + return; + } + } + } +} + +//TODO integrate as mode in main control loop +__attribute__((unused)) void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { + for (;;) { + osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); + update_rotor(motor); + + float phase = get_rotor_phase(motor); + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float v_alpha = c * v_d - s * v_q; + float v_beta = c * v_q + s * v_d; + queue_voltage_timings(motor, v_alpha, v_beta); + + // Check we meet deadlines after queueing + motor->last_cpu_time = check_timing(motor); + if (!(motor->last_cpu_time < motor->control_deadline)) { + motor->error = ERROR_FOC_VOLTAGE_TIMING; + return; + } + } +} + +//-------------------------------- +// Main motor control +//-------------------------------- + +void update_rotor(Motor_t* motor) { + switch (motor->rotor_mode) { + case ROTOR_MODE_ENCODER: + case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: { + //for convenience + Encoder_t* encoder = &motor->encoder; + + // update internal encoder state + int16_t delta_enc = (int16_t)encoder->encoder_timer->Instance->CNT - (int16_t)encoder->encoder_state; + encoder->encoder_state += (int32_t)delta_enc; + + // compute electrical phase + int corrected_enc = encoder->encoder_state % ENCODER_CPR; + corrected_enc -= encoder->encoder_offset; + corrected_enc *= encoder->motor_dir; + float ph = elec_rad_per_enc * (float)corrected_enc; + // ph = fmodf(ph, 2*M_PI); + encoder->phase = wrap_pm_pi(ph); + + // run pll (for now pll is in units of encoder counts) + // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? + // Predict current pos + encoder->pll_pos += current_meas_period * encoder->pll_vel; + // discrete phase detector + float delta_pos = (float)(encoder->encoder_state - (int32_t)floorf(encoder->pll_pos)); + // pll feedback + encoder->pll_pos += current_meas_period * encoder->pll_kp * delta_pos; + encoder->pll_vel += current_meas_period * encoder->pll_ki * delta_pos; + } + // Drop through to sensorless if also testing + if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) + break; + case ROTOR_MODE_SENSORLESS: { + // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer + // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf + // In particular, equation 8 (and by extension eqn 4 and 6). + + // The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle + // is the one computed two cycles ago. To get the correct measurement, it was stored twice: + // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. + + //for convenience + Sensorless_t* sensorless = &motor->sensorless; + + // Clarke transform + float I_alpha_beta[2] = { + -motor->current_meas.phB - motor->current_meas.phC, + one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC)}; + + // alpha-beta vector operations + float eta[2]; + for (int i = 0; i <= 1; ++i) { + // y is the total flux-driving voltage (see paper eqn 4) + float y = -motor->phase_resistance * I_alpha_beta[i] + sensorless->V_alpha_beta_memory[i]; + // flux dynamics (prediction) + float x_dot = y; + // integrate prediction to current timestep + sensorless->flux_state[i] += x_dot * current_meas_period; + + // eta is the estimated permanent magnet flux (see paper eqn 6) + eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; + } + + // Non-linear observer (see paper eqn 8): + float pm_flux_sqr = sensorless->pm_flux_linkage * sensorless->pm_flux_linkage; + float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; + float bandwidth_factor = 1.0f / (sensorless->pm_flux_linkage * sensorless->pm_flux_linkage); + float eta_factor = 0.5f * (sensorless->observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); + + static float eta_factor_avg_test = 0.0f; + eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test); + + // alpha-beta vector operations + for (int i = 0; i <= 1; ++i) { + // add observer action to flux estimate dynamics + float x_dot = eta_factor * eta[i]; + // convert action to discrete-time + sensorless->flux_state[i] += x_dot * current_meas_period; + // update new eta + eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; + } + + // Flux state estimation done, store V_alpha_beta for next timestep + sensorless->V_alpha_beta_memory[0] = motor->current_control.final_v_alpha; + sensorless->V_alpha_beta_memory[1] = motor->current_control.final_v_beta; + + // PLL + // predict PLL phase with velocity + sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_vel); + // update PLL phase with observer permanent magnet phase + sensorless->phase = fast_atan2(eta[1], eta[0]); + float delta_phase = wrap_pm_pi(sensorless->phase - sensorless->pll_pos); + sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_kp * delta_phase); + // update PLL velocity + sensorless->pll_vel += current_meas_period * sensorless->pll_ki * delta_phase; + + //TODO TEMP TEST HACK + // static int trigger_ctr = 0; + // if (++trigger_ctr >= 3*current_meas_hz) { + // trigger_ctr = 0; + + // //Change to sensorless units + // motor->vel_gain = 15.0f / 200.0f; + // motor->vel_setpoint = 800.0f * motor->encoder.motor_dir; + + // //Change mode + // motor->rotor_mode = ROTOR_MODE_SENSORLESS; + // } + + } break; + default: + //TODO error handling + break; + } +} + +bool using_encoder(Motor_t* motor) { + if (motor->rotor_mode == ROTOR_MODE_ENCODER || + motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) + return true; + else + return false; +} + +bool using_sensorless(Motor_t* motor) { + if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) + return true; + else + return false; +} + +float get_rotor_phase(Motor_t* motor) { + if (using_encoder(motor)) + return motor->encoder.phase; + else if (using_sensorless(motor)) + return motor->sensorless.phase; + else + //TODO error handling + return 0.0f; +} + +float get_pll_vel(Motor_t* motor) { + if (using_encoder(motor)) + return motor->encoder.pll_vel; + else if (using_sensorless(motor)) + return motor->sensorless.pll_vel; + else + //TODO error handling + return 0.0f; +} + +// Function that sets the current encoder count to a desired 32-bit value. +void setEncoderCount(Motor_t* motor, uint32_t count) { + // Disable interrupts to make a critical section to avoid race condition + uint32_t prim = __get_PRIMASK(); + __disable_irq(); + motor->encoder.encoder_state = count; + motor->motor_timer->Instance->CNT = count; + motor->encoder.pll_pos = (float)count; + __set_PRIMASK(prim); +} + +bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { + // wait for new timestep + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor->error = ERROR_SPIN_UP_TIMEOUT; + return false; + } + // run estimator + update_rotor(motor); + // override the phase during spinup + motor->sensorless.phase = phase; + // run current control (with the phase override) + FOC_current(motor, I_mag, 0.0f); + + return true; +} + +bool spin_up_sensorless(Motor_t* motor) { + static const float ramp_up_time = 0.4f; + static const float ramp_up_distance = 4 * M_PI; + float ramp_step = current_meas_period / ramp_up_time; + + float phase = 0.0f; + float vel = ramp_up_distance / ramp_up_time; + float I_mag = 0.0f; + + // spiral up current + for (float x = 0.0f; x < 1.0f; x += ramp_step) { + phase = wrap_pm_pi(ramp_up_distance * x); + I_mag = motor->sensorless.spin_up_current * x; + if (!spin_up_timestep(motor, phase, I_mag)) + return false; + } + + // accelerate + while (vel < motor->sensorless.spin_up_target_vel) { + vel += motor->sensorless.spin_up_acceleration * current_meas_period; + phase = wrap_pm_pi(phase + vel * current_meas_period); + if (!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) + return false; + } + + // // test keep spinning + // while (true) { + // phase = wrap_pm_pi(phase + vel * current_meas_period); + // if(!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) + // return false; + // } + + return true; + + // TODO: check pll vel (abs ratio, 0.8) +} + +void update_brake_current(float brake_current) { + if (brake_current < 0.0f) brake_current = 0.0f; + float brake_duty = brake_current * brake_resistance / vbus_voltage; + + // Duty limit at 90% to allow bootstrap caps to charge + if (brake_duty > 0.9f) brake_duty = 0.9f; + int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + + // Safe update of low and high side timings + // To avoid race condition, first reset timings to safe state + // ch3 is low side, ch4 is high side + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + htim2.Instance->CCR3 = low_off; + htim2.Instance->CCR4 = high_on; +} + +void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta) { + float tA, tB, tC; + SVM(mod_alpha, mod_beta, &tA, &tB, &tC); + motor->next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); + motor->next_timings[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); + motor->next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); +} + +void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { + float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); + float mod_alpha = vfactor * v_alpha; + float mod_beta = vfactor * v_beta; + queue_modulation_timings(motor, mod_alpha, mod_beta); +} + +bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { + Current_control_t* ictrl = &motor->current_control; + + // Clarke transform + float Ialpha = -motor->current_meas.phB - motor->current_meas.phC; + float Ibeta = one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC); + + // Park transform + float phase = get_rotor_phase(motor); + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float Id = c * Ialpha + s * Ibeta; + float Iq = c * Ibeta - s * Ialpha; + + // Current error + float Ierr_d = Id_des - Id; + float Ierr_q = Iq_des - Iq; + + // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) + // Apply PI control + float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; + float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; + + float mod_to_V = (2.0f / 3.0f) * vbus_voltage; + float V_to_mod = 1.0f / mod_to_V; + float mod_d = V_to_mod * Vd; + float mod_q = V_to_mod * Vq; + + // Vector modulation saturation, lock integrator if saturated + // TODO make maximum modulation configurable + float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); + if (mod_scalefactor < 1.0f) { + mod_d *= mod_scalefactor; + mod_q *= mod_scalefactor; + // TODO make decayfactor configurable + ictrl->v_current_control_integral_d *= 0.99f; + ictrl->v_current_control_integral_q *= 0.99f; + } else { + ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); + ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); + } + + // Compute estimated bus current + ictrl->Ibus = mod_d * Id + mod_q * Iq; + + // If this is last motor, update brake resistor duty + // if (motor == &motors[num_motors-1]) { + // Above check doesn't work if last motor is executing voltage control + // TODO trigger this update in control_motor_loop instead, + // and make voltage control a control mode in it. + float Ibus_sum = 0.0f; + for (int i = 0; i < num_motors; ++i) { + Ibus_sum += motors[i].current_control.Ibus; + } + // Note: function will clip negative values to 0.0f + update_brake_current(-Ibus_sum); + // } + + // Inverse park transform + float mod_alpha = c * mod_d - s * mod_q; + float mod_beta = c * mod_q + s * mod_d; + + // Report final applied voltage in stationary frame (for sensorles estimator) + ictrl->final_v_alpha = mod_to_V * mod_alpha; + ictrl->final_v_beta = mod_to_V * mod_beta; + + // Apply SVM + queue_modulation_timings(motor, mod_alpha, mod_beta); + + // Check we meet deadlines after queueing + motor->last_cpu_time = check_timing(motor); + if (!(motor->last_cpu_time < motor->control_deadline)) { + motor->error = ERROR_FOC_TIMING; + return false; + } + return true; +} + +void control_motor_loop(Motor_t* motor) { + while (*(motor->axis_legacy.enable_control)) { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; + break; + } + update_rotor(motor); + anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking + + // Position control + // TODO Decide if we want to use encoder or pll position here + float vel_des = motor->vel_setpoint; + if (motor->control_mode >= CTRL_MODE_POSITION_CONTROL) { + if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) { + motor->error = ERROR_POS_CTRL_DURING_SENSORLESS; + break; + } + float pos_err = motor->pos_setpoint - motor->encoder.pll_pos; + vel_des += motor->pos_gain * pos_err; + } + + // Velocity limiting + float vel_lim = motor->vel_limit; + if (vel_des > vel_lim) vel_des = vel_lim; + if (vel_des < -vel_lim) vel_des = -vel_lim; + + // Velocity control + float Iq = motor->current_setpoint; + + // Anti-cogging is enabled after calibration + // We get the current position and apply a current feed-forward + // ensuring that we handle negative encoder positions properly (-1 == ENCODER_CPR - 1) + if (motor->anticogging.use_anticogging) { + Iq += motor->anticogging.cogging_map[mod(motor->encoder.pll_pos, ENCODER_CPR)]; + } + + float v_err = vel_des - get_pll_vel(motor); + if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + Iq += motor->vel_gain * v_err; + } + + // Velocity integral action before limiting + Iq += motor->vel_integrator_current; + + // Apply motor direction correction + if (motor->rotor_mode == ROTOR_MODE_ENCODER || + motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { + Iq *= motor->encoder.motor_dir; + } + + // Current limiting + float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->current_control.max_allowed_current); + bool limited = false; + if (Iq > Ilim) { + limited = true; + Iq = Ilim; + } + if (Iq < -Ilim) { + limited = true; + Iq = -Ilim; + } + + // Velocity integrator (behaviour dependent on limiting) + if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL) { + // reset integral if not in use + motor->vel_integrator_current = 0.0f; + } else { + if (limited) { + // TODO make decayfactor configurable + motor->vel_integrator_current *= 0.99f; + } else { + motor->vel_integrator_current += (motor->vel_integrator_gain * current_meas_period) * v_err; + } + } + + motor->current_control.Iq = Iq; + // Execute current command + if (!FOC_current(motor, 0.0f, Iq)) { + break; // in case of error exit loop, motor->error has been set by FOC_current + } + } + + //We are exiting control, reset Ibus, and update brake current + //TODO update brake current from all motors in 1 func + //TODO reset this motor Ibus, then call from here +} From d88450d84a4d7e3368cca2c7f787ae7efa34ac05 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 14 Dec 2017 22:06:48 -0500 Subject: [PATCH 090/155] Disable index pulse interrupt after it fires --- Firmware/MotorControl/low_level.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 59c86efe..05cff873 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -536,6 +536,11 @@ void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { if (!motors[motor_index].encoder.index_found) { setEncoderCount(&motors[motor_index], 0); motors[motor_index].encoder.index_found = true; + if(motor_index == 0){ + HAL_NVIC_DisableIRQ(EXTI3_IRQn); + } else { + HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); + } } } From 3c424fe81e22ef8ddccc97b24169ee1ff11e48a2 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 14 Dec 2017 22:12:58 -0500 Subject: [PATCH 091/155] Let's use the GPIO Pin for interrupt disabling, it's safer that way --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 05cff873..4176d734 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -536,7 +536,7 @@ void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { if (!motors[motor_index].encoder.index_found) { setEncoderCount(&motors[motor_index], 0); motors[motor_index].encoder.index_found = true; - if(motor_index == 0){ + if(GPIO_Pin == M0_ENC_Z_Pin){ HAL_NVIC_DisableIRQ(EXTI3_IRQn); } else { HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); From 140b303b80b0f395559bf84038e8eee6f03376fd Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 14 Dec 2017 22:15:02 -0500 Subject: [PATCH 092/155] Always disable the index_pin IRQ after trigger --- Firmware/MotorControl/low_level.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 4176d734..e5d449b3 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -536,11 +536,11 @@ void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { if (!motors[motor_index].encoder.index_found) { setEncoderCount(&motors[motor_index], 0); motors[motor_index].encoder.index_found = true; - if(GPIO_Pin == M0_ENC_Z_Pin){ - HAL_NVIC_DisableIRQ(EXTI3_IRQn); - } else { - HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); - } + } + if(GPIO_Pin == M0_ENC_Z_Pin){ + HAL_NVIC_DisableIRQ(EXTI3_IRQn); + } else { + HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); } } From 5aec8667884912cdd035e68c732b139325dbcfc1 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 15 Dec 2017 19:51:51 -0800 Subject: [PATCH 093/155] check DRV fault --- Firmware/MotorControl/low_level.c | 43 +++++++++++++++++++------------ Firmware/MotorControl/low_level.h | 4 ++- 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 3f906834..8383b0be 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -320,7 +320,7 @@ void global_fault(int error) { *(motors[i].axis_legacy.enable_control) = false; } // disable brake resistor - update_brake_current(0.0f); + set_brake_current(0.0f); } float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) { @@ -1112,7 +1112,16 @@ bool spin_up_sensorless(Motor_t* motor) { // TODO: check pll vel (abs ratio, 0.8) } -void update_brake_current(float brake_current) { +void update_brake_current() { + float Ibus_sum = 0.0f; + for (int i = 0; i < num_motors; ++i) { + Ibus_sum += motors[i].current_control.Ibus; + } + // Note: set_brake_current will clip negative values to 0.0f + set_brake_current(-Ibus_sum); +} + +void set_brake_current(float brake_current) { if (brake_current < 0.0f) brake_current = 0.0f; float brake_duty = brake_current * brake_resistance / vbus_voltage; @@ -1191,19 +1200,6 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Compute estimated bus current ictrl->Ibus = mod_d * Id + mod_q * Iq; - // If this is last motor, update brake resistor duty - // if (motor == &motors[num_motors-1]) { - // Above check doesn't work if last motor is executing voltage control - // TODO trigger this update in control_motor_loop instead, - // and make voltage control a control mode in it. - float Ibus_sum = 0.0f; - for (int i = 0; i < num_motors; ++i) { - Ibus_sum += motors[i].current_control.Ibus; - } - // Note: function will clip negative values to 0.0f - update_brake_current(-Ibus_sum); - // } - // Inverse park transform float mod_alpha = c * mod_d - s * mod_q; float mod_beta = c * mod_q + s * mod_d; @@ -1224,12 +1220,23 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { return true; } +//Returns true if the fault line is asserted +bool check_DRV_fault(Motor_t* motor) { + //TODO: make this pin configurable per motor ch + GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(nFAULT_GPIO_Port, nFAULT_Pin); + return (nFAULT_state == GPIO_PIN_RESET) ? true : false; +} + void control_motor_loop(Motor_t* motor) { while (*(motor->axis_legacy.enable_control)) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; break; } + if (check_DRV_fault(motor)) { + motor->error = ERROR_DRV_FAULT; + break; + } update_rotor(motor); anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking @@ -1304,9 +1311,11 @@ void control_motor_loop(Motor_t* motor) { if (!FOC_current(motor, 0.0f, Iq)) { break; // in case of error exit loop, motor->error has been set by FOC_current } + + update_brake_current(); } //We are exiting control, reset Ibus, and update brake current - //TODO update brake current from all motors in 1 func - //TODO reset this motor Ibus, then call from here + motor->current_control.Ibus = 0.0f; + update_brake_current(); } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 342c19ec..22a09877 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -48,6 +48,7 @@ typedef enum { ERROR_UNEXPECTED_STEP_SRC, ERROR_POS_CTRL_DURING_SENSORLESS, ERROR_SPIN_UP_TIMEOUT, + ERROR_DRV_FAULT, } Error_t; // Note: these should be sorted from lowest level of control to @@ -232,7 +233,8 @@ bool using_sensorless(Motor_t* motor); float get_rotor_phase(Motor_t* motor); float get_pll_vel(Motor_t* motor); bool spin_up_sensorless(Motor_t* motor); -void update_brake_current(float brake_current); +void update_brake_current(); +void set_brake_current(float brake_current); void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta); void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta); bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); From 07525383b29aa74e7b16816e6084182876a88d6f Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 15 Dec 2017 20:18:43 -0800 Subject: [PATCH 094/155] boost switching freq, add vbus_s HV define --- Firmware/Inc/main.h | 10 +++++++++- Firmware/MotorControl/low_level.c | 2 +- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index ef2b2734..2a26800c 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -54,6 +54,7 @@ #define HW_VERSION_MAJOR 3 #define HW_VERSION_MINOR 4 +// #define HW_VERSION_HIGH_VOLTAGE true #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 @@ -63,7 +64,8 @@ /* Private define ------------------------------------------------------------*/ #define TIM_1_8_CLOCK_HZ 168000000 -#define TIM_1_8_PERIOD_CLOCKS 10192 +// #define TIM_1_8_PERIOD_CLOCKS 8192 +#define TIM_1_8_PERIOD_CLOCKS 6000 #define TIM_1_8_DEADTIME_CLOCKS 20 #define TIM_APB1_CLOCK_HZ 84000000 #define TIM_APB1_PERIOD_CLOCKS 4096 @@ -157,6 +159,12 @@ #define CURRENT_MEAS_PERIOD ((float)(2*TIM_1_8_PERIOD_CLOCKS)/(float)TIM_1_8_CLOCK_HZ) #define CURRENT_MEAS_HZ (TIM_1_8_CLOCK_HZ/(2*TIM_1_8_PERIOD_CLOCKS)) +#if HW_VERSION_HIGH_VOLTAGE == true +#define VBUS_S_DIVIDER_RATIO 19.0f +#else +#define VBUS_S_DIVIDER_RATIO 11.0f +#endif + /* USER CODE END Private defines */ void _Error_Handler(char *, int); diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 8383b0be..3de25b22 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -528,7 +528,7 @@ void step_cb(uint16_t GPIO_Pin) { } void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * 11.0f / (float)(1 << 12); + static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12); // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); vbus_voltage = ADCValue * voltage_scale; From 9287f5a00b1c4a425c3dfd268db9875b870405eb Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 18 Dec 2017 15:20:31 -0800 Subject: [PATCH 095/155] add hw v3.4, check DRV fault --- Firmware/CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index da7b613f..eeb1d1a4 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -7,9 +7,12 @@ * Support for C++ * Demo scripts for getting started with commanding ODrive from python * Protection from user setting current_lim higher than is measurable +* Current sense shunt values for HW v3.4 +* Check DRV chip fault line ### Changed * Shunt resistance values for v3.3 and earlier to include extra resistance of PCB +* Default HW revision to v3.4 * Refactoring of control code: * Lifted top layer of low_level.c into Axis.cpp From 138ad2037aae8baa1f4b2bb6f873aca254ff750e Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 18 Dec 2017 15:22:29 -0800 Subject: [PATCH 096/155] release v0.3 date in changelog --- Firmware/CHANGELOG.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index eeb1d1a4..7ddb3bba 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,5 +1,4 @@ -## [0.3] - +## [0.3] - 2017-12-18 ### Added * **New binary communication protocol** * This is a much richer and more efficient binary protocol than the old human-readable protocol. From de5d5340fc0edf4094360e61b2a035ecf4e5b5ef Mon Sep 17 00:00:00 2001 From: vfdev Date: Tue, 19 Dec 2017 00:52:17 +0100 Subject: [PATCH 097/155] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 22b83ed3..ed96cb4b 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ This project is all about accurately driving brushless motors, for cheap. The ai ## Getting Started It is perfectly fine, and even recommended, to start testing with just a single motor and encoder. -Make sure you have a good mechanical connection between the encdoer and the motor, slip can cause disasterous oscillations. +Make sure you have a good mechanical connection between the encoder and the motor, slip can cause disasterous oscillations. All non-power I/O is 3.3V output and 5V tolerant on input, except: * GPIO 3 and GPIO 4 are NOT 5V tolerant on ODrive v3.2 and earlier. From 3dd6683f67f932ee3d444f94c1c90910e2758dca Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 19 Dec 2017 19:16:04 -0800 Subject: [PATCH 098/155] Update README.md --- Firmware/README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Firmware/README.md b/Firmware/README.md index 5fda6924..fa667243 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -25,6 +25,10 @@ The first thing to set is your board hardware version, located at the top of [In #define HW_VERSION_MAJOR 3 #define HW_VERSION_MINOR 2 ``` +If you are using the 48V version of ODrive, you should also uncomment this line +```C +#define HW_VERSION_HIGH_VOLTAGE true +``` ### Communication configuration If want to use the example python scripts and connect the ODrive via USB, the defaults are fine for you and you can skip this step. From c14bc7b87152df481dc4ee162ff456157df3682c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 19 Dec 2017 20:56:36 -0800 Subject: [PATCH 099/155] add Iq control effort report --- Firmware/MotorControl/commands.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 54031438..bc426507 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -123,6 +123,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("i_gain", &motors[0].current_control.i_gain), Endpoint::make_property("v_current_control_integral_d", &motors[0].current_control.v_current_control_integral_d), Endpoint::make_property("v_current_control_integral_q", &motors[0].current_control.v_current_control_integral_q), + Endpoint::make_property("Iq_command", &motors[0].current_control.Iq), Endpoint::make_property("Ibus", const_cast(&motors[0].current_control.Ibus)), Endpoint::close_tree(), Endpoint::make_object("encoder"), @@ -176,6 +177,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("i_gain", &motors[1].current_control.i_gain), Endpoint::make_property("v_current_control_integral_d", &motors[1].current_control.v_current_control_integral_d), Endpoint::make_property("v_current_control_integral_q", &motors[1].current_control.v_current_control_integral_q), + Endpoint::make_property("Iq_command", &motors[1].current_control.Iq), Endpoint::make_property("Ibus", const_cast(&motors[1].current_control.Ibus)), Endpoint::close_tree(), Endpoint::make_object("encoder"), From 19548f5642b67a39cdbfce6c8c380cdc7f3b772d Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 21 Dec 2017 10:19:42 -0800 Subject: [PATCH 100/155] Update README.md --- Firmware/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index fa667243..518cbc62 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -71,8 +71,8 @@ You must set: ### Tuning parameters The most important parameters are the limits: * The current limit: `.current_lim = 75.0f, //[A] // Note: consistent with 40v/v gain`. The default current limit, for safety reasons, is set to 10A. This is quite weak, and good for making sure the drive is stable. Once you have tuned the drive, you can increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains. -* The velocity limit: `.vel_limit = 20000.0f, // [counts/s]`. The motor will be limited to this speed; again the default value is quite slow. * Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current. +* The velocity limit: `.vel_limit = 20000.0f, // [counts/s]`. The motor will be limited to this speed; again the default value is quite slow. The motion control gains are currently manually tuned: * `.pos_gain = 20.0f, // [(counts/s) / counts]` From 9ed3f7302ddfc92dec7e3c2eaafac4a375183d06 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 22 Dec 2017 20:53:33 -0800 Subject: [PATCH 101/155] Change PWM period from 6k to 8k clocks --- Firmware/Inc/main.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index 2a26800c..228c6018 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -64,8 +64,7 @@ /* Private define ------------------------------------------------------------*/ #define TIM_1_8_CLOCK_HZ 168000000 -// #define TIM_1_8_PERIOD_CLOCKS 8192 -#define TIM_1_8_PERIOD_CLOCKS 6000 +#define TIM_1_8_PERIOD_CLOCKS 8192 #define TIM_1_8_DEADTIME_CLOCKS 20 #define TIM_APB1_CLOCK_HZ 84000000 #define TIM_APB1_PERIOD_CLOCKS 4096 From 66431f2176cdd986e22b9427f0eea0d75619af6d Mon Sep 17 00:00:00 2001 From: Quincy Jones Date: Sat, 23 Dec 2017 01:39:21 -0600 Subject: [PATCH 102/155] Changed #ifdef logic for VBUS divider ratio selection. --- Firmware/Inc/main.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index 2a26800c..7bbe9ae7 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -159,7 +159,7 @@ #define CURRENT_MEAS_PERIOD ((float)(2*TIM_1_8_PERIOD_CLOCKS)/(float)TIM_1_8_CLOCK_HZ) #define CURRENT_MEAS_HZ (TIM_1_8_CLOCK_HZ/(2*TIM_1_8_PERIOD_CLOCKS)) -#if HW_VERSION_HIGH_VOLTAGE == true +#ifdef HW_VERSION_HIGH_VOLTAGE #define VBUS_S_DIVIDER_RATIO 19.0f #else #define VBUS_S_DIVIDER_RATIO 11.0f From 7b0a410d137c82dcb2b376acafb60a8198c2fc6b Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 24 Dec 2017 13:56:55 -0800 Subject: [PATCH 103/155] Change HW_VERSION_HIGH_VOLTAGE on main to ifdef --- Firmware/Inc/main.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index 228c6018..a9650093 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -158,7 +158,7 @@ #define CURRENT_MEAS_PERIOD ((float)(2*TIM_1_8_PERIOD_CLOCKS)/(float)TIM_1_8_CLOCK_HZ) #define CURRENT_MEAS_HZ (TIM_1_8_CLOCK_HZ/(2*TIM_1_8_PERIOD_CLOCKS)) -#if HW_VERSION_HIGH_VOLTAGE == true +#ifdef HW_VERSION_HIGH_VOLTAGE #define VBUS_S_DIVIDER_RATIO 19.0f #else #define VBUS_S_DIVIDER_RATIO 11.0f From 10685d5be92809b65f7e76c5a6a6c2a8ec87c136 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 27 Dec 2017 10:01:00 -0800 Subject: [PATCH 104/155] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index ed96cb4b..dacfbb6c 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ You need one or two [brushless motors](https://hackaday.io/project/11583-odrive- Wire up the motor phases into the 3-phase screw terminals, and the power resistor to the AUX terminal. Wire up the power source (12-24V) to the DC terminal, make sure to pay attention to the polarity. Do not apply power just yet. -Wire up the encoder(s) to J4. The A,B phases are required, and the Z (index pulse) is optional. The A,B and Z lines have 1k pull up resistors, for use with open-drain encoder outputs. For single ended push-pull signals with weak drive current (\<4mA), you may want to desolder the pull-ups. +Wire up the encoder(s) to J4. The A,B phases are required, and the Z (index pulse) is optional. The A,B and Z lines have 3.3k pull up resistors, for use with open-drain encoder outputs. For single ended push-pull signals with weak drive current (\<4mA), you may want to desolder the pull-ups. ![Image of ODrive all hooked up](https://docs.google.com/drawings/d/e/2PACX-1vTCD0P40Cd-wvD7Fl8UYEaxp3_UL81oI4qUVqrrCJPi6tkJeSs2rsffIXQRpdu6rNZs6-2mRKKYtILG/pub?w=1716&h=1281) From ec050387e2e5d905fad97703903270409a732b89 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 30 Dec 2017 01:00:34 -0800 Subject: [PATCH 105/155] Update main.h --- Firmware/Inc/main.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index 7bbe9ae7..a9650093 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -64,8 +64,7 @@ /* Private define ------------------------------------------------------------*/ #define TIM_1_8_CLOCK_HZ 168000000 -// #define TIM_1_8_PERIOD_CLOCKS 8192 -#define TIM_1_8_PERIOD_CLOCKS 6000 +#define TIM_1_8_PERIOD_CLOCKS 8192 #define TIM_1_8_DEADTIME_CLOCKS 20 #define TIM_APB1_CLOCK_HZ 84000000 #define TIM_APB1_PERIOD_CLOCKS 4096 From 892646bff69197585eb041812d91606ee49ea515 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 2 Jan 2018 15:41:16 -0800 Subject: [PATCH 106/155] Update README.md --- Firmware/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/README.md b/Firmware/README.md index 518cbc62..9f00c1bf 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -4,6 +4,8 @@ If you wish to use the latest release, please use the `master` branch (this is t If you are a developer, you are encouraged to use the `devel` branch, as it contains the latest features. +The project is under active development, so make sure to check the [Changelog](CHANGELOG.md) to keep track of updates. + ### Table of contents From e2a8dce3553595fc39858fcb72e5ace7324c54c0 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 2 Jan 2018 20:52:40 -0800 Subject: [PATCH 107/155] Update low_level.c --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 3de25b22..ecf23231 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -418,7 +418,7 @@ void start_adc_pwm() { start_pwm(&htim1); start_pwm(&htim8); // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); + sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 0 * 128); // Motor output starts in the disabled state __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); From 153d0667ee81d4852b6f4616c571304611b71e8f Mon Sep 17 00:00:00 2001 From: beak90 Date: Wed, 10 Jan 2018 17:21:29 -0800 Subject: [PATCH 108/155] Changed settings for my setup --- Firmware/MotorControl/commands.h | 4 ++-- Firmware/MotorControl/low_level.c | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 0d9d7c87..07d46423 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -11,8 +11,8 @@ #include "crc.hpp" // Select which protocol to run on USB (see README for more details) -#define USB_PROTOCOL_NATIVE -// #define USB_PROTOCOL_NATIVE_STREAM_BASED +// #define USB_PROTOCOL_NATIVE +#define USB_PROTOCOL_NATIVE_STREAM_BASED // #define USB_PROTOCOL_LEGACY // #define USB_PROTOCOL_NONE diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 3de25b22..c29a45f2 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -34,7 +34,7 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (600 * 4) +#define ENCODER_CPR (500 * 4) #define POLE_PAIRS 7 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); @@ -56,15 +56,15 @@ Motor_t motors[] = { .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] + .pos_gain = 40.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, // .vel_setpoint = 800.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 25.0f / 10000.0f, // [A/(counts/s)] // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] + .vel_limit = 200000.0f, // [counts/s] .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] .phase_inductance = 0.0f, // to be set by measure_phase_inductance @@ -97,7 +97,7 @@ Motor_t motors[] = { // Read out max_allowed_current to see max supported value for current_lim. // You can change DRV8301_ShuntAmpGain to get a different range. // .current_lim = 75.0f, //[A] - .current_lim = 10.0f, //[A] + .current_lim = 20.0f, //[A] .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, From 869467a0d7b975f9d1b6671194a5f8060a2179ab Mon Sep 17 00:00:00 2001 From: beak90 Date: Wed, 10 Jan 2018 18:32:08 -0800 Subject: [PATCH 109/155] Added MacOS vscode cpp configuration with the proper paths for the gcc-arm-embedded package when installed with Homebrew --- Firmware/.vscode/c_cpp_properties.json | 46 ++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index feba750c..f732a2df 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -89,6 +89,52 @@ "limitSymbolsToIncludedHeaders": true, "databaseFilename": "" } + }, + { + "name": "MacOS", + "includePath": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl", + "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include", + "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include/c++/6.3.1", + "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include/c++/6.3.1/arm-none-eabi", + "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/lib/gcc/arm-none-eabi/6.3.1/include" + ], + "defines": [ + "_DEBUG", + "UNICODE" + ], + "intelliSenseMode": "msvc-x64", + "browse": { + "path": [ + "${workspaceRoot}", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/include", + "${workspaceRoot}/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", + "${workspaceRoot}/Drivers/DRV8301", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Core/Inc", + "${workspaceRoot}/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc", + "${workspaceRoot}/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", + "${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include", + "${workspaceRoot}/Drivers/CMSIS/Include", + "${workspaceRoot}/Inc", + "${workspaceRoot}/MotorControl" + ], + "limitSymbolsToIncludedHeaders": true, + "databaseFilename": "" + } } ], "version": 3 From af155b8d26eaf2218e7eca721c5ea67eaf796618 Mon Sep 17 00:00:00 2001 From: beak90 Date: Wed, 10 Jan 2018 18:43:58 -0800 Subject: [PATCH 110/155] Revert "Changed settings for my setup" This reverts commit 153d0667ee81d4852b6f4616c571304611b71e8f. --- Firmware/MotorControl/commands.h | 4 ++-- Firmware/MotorControl/low_level.c | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 07d46423..0d9d7c87 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -11,8 +11,8 @@ #include "crc.hpp" // Select which protocol to run on USB (see README for more details) -// #define USB_PROTOCOL_NATIVE -#define USB_PROTOCOL_NATIVE_STREAM_BASED +#define USB_PROTOCOL_NATIVE +// #define USB_PROTOCOL_NATIVE_STREAM_BASED // #define USB_PROTOCOL_LEGACY // #define USB_PROTOCOL_NONE diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index c29a45f2..3de25b22 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -34,7 +34,7 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (500 * 4) +#define ENCODER_CPR (600 * 4) #define POLE_PAIRS 7 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); @@ -56,15 +56,15 @@ Motor_t motors[] = { .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 40.0f, // [(counts/s) / counts] + .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, // .vel_setpoint = 800.0f, - .vel_gain = 25.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] .vel_integrator_current = 0.0f, // [A] - .vel_limit = 200000.0f, // [counts/s] + .vel_limit = 20000.0f, // [counts/s] .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] .phase_inductance = 0.0f, // to be set by measure_phase_inductance @@ -97,7 +97,7 @@ Motor_t motors[] = { // Read out max_allowed_current to see max supported value for current_lim. // You can change DRV8301_ShuntAmpGain to get a different range. // .current_lim = 75.0f, //[A] - .current_lim = 20.0f, //[A] + .current_lim = 10.0f, //[A] .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, From fc891c418db169225fd798d8e1825d5d4fecac79 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 11 Jan 2018 17:41:29 -0800 Subject: [PATCH 111/155] Update protocol.py --- tools/odrive/protocol.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index bef172d5..1f8c9114 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -181,7 +181,7 @@ class Channel(PacketSink): _expected_acks = {} # Choose these parameters to be sensible for a specific transport layer - _resend_timeout = 5.0 # [s] + _resend_timeout = 0.1 # [s] _send_attempts = 5 def __init__(self, name, input, output): From 84d7ced0430f7e7dfb8b4569d16df1bf156e9fe5 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 13 Jan 2018 12:25:59 -0500 Subject: [PATCH 112/155] Add Cortex-Debug debugging configuration --- Firmware/.vscode/launch.json | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index f6e36562..e77ffee8 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -4,6 +4,20 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + { + // For the Cortex-Debug extension + "type": "openocd-gdb", + "request": "launch", + "name": "Debug Microcontroller", + "gdbpath": "arm-none-eabi-gdb", + "executable": "${workspaceRoot}/build/ODriveFirmware.elf", + "configFiles": [ + "interface/stlink-v2.cfg", + "target/stm32f4x_stlink.cfg", + ], + "cwd": "${workspaceRoot}" + }, + // For the Native Debug extension { "type": "gdb", "request": "attach", @@ -14,7 +28,6 @@ "executable": "./build/ODriveFirmware.elf", "cwd": "${workspaceRoot}", "printCalls": false, - //"preLaunchTask": "openocd", // This isn't working quite right. "autorun": [ "monitor reset halt" ] From 14832e1ab09bb31dcf323df134384995a11c50d1 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 13 Jan 2018 12:37:52 -0500 Subject: [PATCH 113/155] Change VSCode documentation to reflect Cortex-Debug --- Firmware/configuring-vscode.md | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/Firmware/configuring-vscode.md b/Firmware/configuring-vscode.md index 62368874..0b2950c8 100644 --- a/Firmware/configuring-vscode.md +++ b/Firmware/configuring-vscode.md @@ -11,8 +11,8 @@ Before doing the VSCode setup, make sure you've installed all of your [prerequis 1. Install extensions. This can be done directly from VSCode (Ctrl+Shift+X) * Required extensions: * C/C++ - * Native Debug * Recommended Extensions: + * Cortex-Debug * vscode-icons * Code Outline * Include Autocomplete @@ -36,16 +36,17 @@ A terminal window will open with your native shell. VSCode is configured to run If the flashing worked, you can start sending commands. If you want to do that now, you can go to [Communicating over USB or UART](README.md#communicating-over-usb-or-uart). ## Debugging -The solution we have is not the most elegant, and if you know a better way, please do help us. +An extension called Cortex-Debug has recently been released which is designed specifically for debugging ARM Cortex projects. You can read more on Cortex-Debug here: https://github.com/Marus/cortex-debug + +Note: If developing on Windows, you should have `arm-none-eabi-gdb` and `openOCD` on your PATH. + * Make sure you have the Firmware folder as your active folder * Flash the board with the newest code (starting debug session doesn't do this) - * Tasks -> Run Task -> openocd - * Debug -> Start Debugging + * Debug -> Start Debugging (or press F5) * The processor will reset and halt. * Set your breakpoints. Note: you can only set breakpoints when the processor is halted, if you set them during run mode, they won't get applied. * Run - * When you are done, you must kill the openocd task before you are able to flash the board again: - * Tasks -> Terminate task -> openocd + * When done debugging, simply halt the debugger. It will kill your ## Cleaning the Build This sometimes needs to be done if you change branches. From 0376f3993d9d98a8f63134373b1883ec78ade044 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 13 Jan 2018 12:38:37 -0500 Subject: [PATCH 114/155] Remove Native Debug configuration --- Firmware/.vscode/launch.json | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index e77ffee8..38053439 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -17,20 +17,5 @@ ], "cwd": "${workspaceRoot}" }, - // For the Native Debug extension - { - "type": "gdb", - "request": "attach", - "name": "Debug Firmware", - "target": "localhost:3333", - "gdbpath": "arm-none-eabi-gdb", - "remote": true, - "executable": "./build/ODriveFirmware.elf", - "cwd": "${workspaceRoot}", - "printCalls": false, - "autorun": [ - "monitor reset halt" - ] - } ] } \ No newline at end of file From 85a802dc4cec0b17d6cd3e74234bad504e0591fd Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 13 Jan 2018 12:39:08 -0500 Subject: [PATCH 115/155] Remove gdbPath variable. Cortex-Debug already handles it --- Firmware/.vscode/launch.json | 1 - 1 file changed, 1 deletion(-) diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index 38053439..6cd5be0b 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -9,7 +9,6 @@ "type": "openocd-gdb", "request": "launch", "name": "Debug Microcontroller", - "gdbpath": "arm-none-eabi-gdb", "executable": "${workspaceRoot}/build/ODriveFirmware.elf", "configFiles": [ "interface/stlink-v2.cfg", From 05b374fb18e9fb205f7d4c6044e4adc1bf7b4f03 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 13 Jan 2018 12:40:56 -0500 Subject: [PATCH 116/155] Rename the debug config to Debug ODrive --- Firmware/.vscode/launch.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index 6cd5be0b..747e4f1a 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -8,7 +8,7 @@ // For the Cortex-Debug extension "type": "openocd-gdb", "request": "launch", - "name": "Debug Microcontroller", + "name": "Debug ODrive", "executable": "${workspaceRoot}/build/ODriveFirmware.elf", "configFiles": [ "interface/stlink-v2.cfg", From 06977066d1c06689817d82d82dab2318e96fe608 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 13 Jan 2018 12:54:37 -0500 Subject: [PATCH 117/155] Add more info on controlling the debugger --- Firmware/configuring-vscode.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Firmware/configuring-vscode.md b/Firmware/configuring-vscode.md index 0b2950c8..ec9b0adb 100644 --- a/Firmware/configuring-vscode.md +++ b/Firmware/configuring-vscode.md @@ -45,8 +45,9 @@ Note: If developing on Windows, you should have `arm-none-eabi-gdb` and `openOCD * Debug -> Start Debugging (or press F5) * The processor will reset and halt. * Set your breakpoints. Note: you can only set breakpoints when the processor is halted, if you set them during run mode, they won't get applied. - * Run - * When done debugging, simply halt the debugger. It will kill your + * Run (F5) + * Stepping over/in/out, restarting, and changing breakpoints can be done by first pressing the "pause" (F6) button at the top the screen. + * When done debugging, simply stop (Shift+F5) the debugger. It will kill your openOCD process too. ## Cleaning the Build This sometimes needs to be done if you change branches. From 6cefbf6e62db74eadc34712c62279dd376977b51 Mon Sep 17 00:00:00 2001 From: Cristian Fluture Date: Sun, 14 Jan 2018 23:38:21 -0800 Subject: [PATCH 118/155] Added 3 endpoints (UUID_0, UUID_1, UUID2) to expose the device UUID --- Firmware/MotorControl/commands.cpp | 3 +++ Firmware/MotorControl/protocol.hpp | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index bc426507..25871230 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -94,6 +94,9 @@ void motors_1_set_current_setpoint_func(void) { const Endpoint endpoints[] = { Endpoint::make_property("vbus_voltage", const_cast(&vbus_voltage)), Endpoint::make_property("elec_rad_per_enc", const_cast(&elec_rad_per_enc)), + Endpoint::make_property("UUID_0", (const uint32_t*)(ID_UNIQUE_ADDRESS + 0*4)), + Endpoint::make_property("UUID_1", (const uint32_t*)(ID_UNIQUE_ADDRESS + 1*4)), + Endpoint::make_property("UUID_2", (const uint32_t*)(ID_UNIQUE_ADDRESS + 2*4)), Endpoint::make_object("motor0"), Endpoint::make_property("control_mode", reinterpret_cast(&motors[0].control_mode)), Endpoint::make_property("error", reinterpret_cast(&motors[0].error)), diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index 92664d55..bc3b3b9a 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -335,6 +335,10 @@ inline const char* get_default_json_modifier() { return "\"type\":\"int32\",\"access\":\"rw\""; } template<> +inline const char* get_default_json_modifier() { + return "\"type\":\"uint32\",\"access\":\"r\""; +} +template<> inline const char* get_default_json_modifier() { return "\"type\":\"uint16\",\"access\":\"r\""; } From e7398e23c5504d8c8ab1e8521fcf535d7b399501 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 16 Jan 2018 19:36:05 -0800 Subject: [PATCH 119/155] Update low_level.c --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index ecf23231..3de25b22 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -418,7 +418,7 @@ void start_adc_pwm() { start_pwm(&htim1); start_pwm(&htim8); // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 0 * 128); + sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); // Motor output starts in the disabled state __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); From d898caede7933b7c67429196698b730be95a7caf Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 16 Jan 2018 23:01:14 -0800 Subject: [PATCH 120/155] improve python on windows instructions --- Firmware/README.md | 38 +++++++++++++++++--------------------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/Firmware/README.md b/Firmware/README.md index 679656bb..f2dd9873 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -148,39 +148,35 @@ The following options are known to work and supported: ## Communicating over USB or UART Warning: If testing USB or UART communication for the first time it is recommend that your motors are free to spin continuously and are not connected to a drivetrain with limited travel. ### From Linux/Windows/macOS -There are two simple python scripts to help you get started with controlling the ODrive using python. +There are two example python scripts to help you get started with controlling the ODrive using python. One will drop you into an interactive shell to query settings, parameters, and variables, and let you send setpoints manually ([tools/explore_odrive.py](tools/explore_odrive.py)). The other is a demo application to show you how to control the ODrive programmatically ([tools/demo.py](tools/demo.py)). Below follows a step-by-step guide on how to run these. + + +* __Windows__: It is recommended to use a Unix style command prompt, such as Git Bash that comes with [Git for windows](https://git-scm.com/download/win). 1. [Install Python 3](https://www.python.org/downloads/), then install dependencies pyusb and pyserial: - * __Linux__ ``` pip install pyusb pyserial ``` - * __Windows__ -From the start menu type 'cmd' and open the command prompt. If you only have python3 installed then enter: -``` -pip install pyusb pyserial -``` -If you have python2 and python3 installed concurrently then you must specifiy the location of pip for python3. For me this was at 'C:\Users\ 'username' \AppData\Local\Programs\Python\Python36-32\Scripts\' and so I instead enter: -``` -C:\Users\'username'\AppData\Local\Programs\Python\Python36-32\Scripts\pip install pyusb pyserial -``` -If you have trouble with this step then refer to [this walkthrough.](https://www.youtube.com/watch?v=jnpC_Ib_lbc) +* Note: If you have python2 and python3 installed concurrently then you must specifiy that we wish to target python3. This is done as follows: + * __Linux__: Use `pip3` instead of `pip` in the above command. + * __Windows__: Use the full path of the Python3 pip, yeilding something like: + `C:\Users\YOUR_USERNAME\AppData\Local\Programs\Python\Python36-32\Scripts\pip install pyusb pyserial` +* If you have trouble with this step then refer to [this walkthrough.](https://www.youtube.com/watch?v=jnpC_Ib_lbc) -3. __Linux__: set up USB permissions +2. __Linux__: set up USB permissions ``` echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d[0-9][0-9]", MODE="0666"' | sudo tee /etc/udev/rules.d/50-odrive.rules sudo udevadm control --reload-rules sudo udevadm trigger # until you reboot you may need to do this everytime you reset the ODrive ``` -4. Power the ODrive board (as per the [Flashing the firmware](#flashing-the-firmware) step) -5. Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC -6. __Windows__: Use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb. +3. Power the ODrive board (as per the [Flashing the firmware](#flashing-the-firmware) step). +4. Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC. +5. __Windows__: Use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb-win32. * If 'Odrive V3.x' is not in the list of devices upon opening Zadig, check 'List All Devices' from the options menu. With the Odrive selected in the device list choose 'libusb-win32' from the target driver list and select the large 'install driver' button. -7. Run `./tools/demo.py` or `./tools/explore_odrive.py`. - - `demo.py` is a very simple script which will make motor 0 turn back and forth. Use this as an example if you want to control the ODrive yourself programatically. - - `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --discover serial`. - * __Windows Users:__ -If you run either of the python scripts and only see a prompt window appear for a split second before it closes then it is likely that you have not installed pyusb and pyserial for python3 correctly. +6. Open the bash prompt in the `ODrive/tools/` folder. +7. Run `python3 demo.py` or `python3 explore_odrive.py`. +- `demo.py` is a very simple script which will make motor 0 turn back and forth. Use this as an example if you want to control the ODrive yourself programatically. +- `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --discover serial`. ### From Arduino [See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) From 2d0ab777342f1b2e167e0bd80998d7a9210a587c Mon Sep 17 00:00:00 2001 From: fluture Date: Tue, 16 Jan 2018 23:31:44 -0800 Subject: [PATCH 121/155] Fixed sending all commands to motor 0 --- Firmware/MotorControl/commands.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 25871230..ed9a263c 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -73,18 +73,18 @@ void motors_0_set_current_setpoint_func(void) { motors[0].set_current_setpoint_args.current_setpoint); } void motors_1_set_pos_setpoint_func(void) { - set_pos_setpoint(&motors[0], + set_pos_setpoint(&motors[1], motors[1].set_pos_setpoint_args.pos_setpoint, motors[1].set_pos_setpoint_args.vel_feed_forward, motors[1].set_pos_setpoint_args.current_feed_forward); } void motors_1_set_vel_setpoint_func(void) { - set_vel_setpoint(&motors[0], + set_vel_setpoint(&motors[1], motors[1].set_vel_setpoint_args.vel_setpoint, motors[1].set_vel_setpoint_args.current_feed_forward); } void motors_1_set_current_setpoint_func(void) { - set_current_setpoint(&motors[0], + set_current_setpoint(&motors[1], motors[1].set_current_setpoint_args.current_setpoint); } From d8b649d8f075f53dbd3c8a181c83772fd051db54 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 17 Jan 2018 23:47:31 -0800 Subject: [PATCH 122/155] update CHANGELOG.md --- Firmware/CHANGELOG.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 7ddb3bba..4f95421e 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,3 +1,12 @@ +## UNRELEASED + +### Added +* Getting started instructions for VSCode + +### Changed +* Recommended method to debug firmware from VSCode now uses Cortex-Debug extension instead of native-debug. +* Refactor IDE instructions into separate files + ## [0.3] - 2017-12-18 ### Added * **New binary communication protocol** From 96fbc9bab75fba34a2d8f4e22e165e346de5cf98 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 18 Jan 2018 00:00:27 -0800 Subject: [PATCH 123/155] update changelog --- Firmware/CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 4f95421e..92773f68 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,12 +1,16 @@ ## UNRELEASED ### Added +* UUID Endpoint * Getting started instructions for VSCode ### Changed * Recommended method to debug firmware from VSCode now uses Cortex-Debug extension instead of native-debug. * Refactor IDE instructions into separate files +### Fixed +* Bug where the remote function calls from Python to the ODrive were not working properly. + ## [0.3] - 2017-12-18 ### Added * **New binary communication protocol** From 92abe9290a08381c492311aaae92ff5309ec2b70 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 18 Jan 2018 00:18:01 -0800 Subject: [PATCH 124/155] update USB descriptor --- Firmware/Src/usbd_desc.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/Firmware/Src/usbd_desc.c b/Firmware/Src/usbd_desc.c index d98a872c..380e06d6 100644 --- a/Firmware/Src/usbd_desc.c +++ b/Firmware/Src/usbd_desc.c @@ -70,17 +70,17 @@ /** @defgroup USBD_DESC_Private_Defines * @{ - */ -#define USBD_VID 0x1209 -#define USBD_LANGID_STRING 1033 -#define USBD_MANUFACTURER_STRING "ODrive" -#define USBD_PID_FS 0x0D31 -#define USBD_PRODUCT_XSTR(s) USBD_PRODUCT_STR(s) -#define USBD_PRODUCT_STR(s) #s -#define USBD_PRODUCT_STRING_FS ODrive HW_VERSION_MAJOR.HW_VERSION_MINOR -#define USBD_SERIALNUMBER_STRING_FS "000000000001" -#define USBD_CONFIGURATION_STRING_FS "CDC Config" -#define USBD_INTERFACE_STRING_FS "CDC Interface" + */ +#define USBD_VID 0x1209 +#define USBD_LANGID_STRING 1033 +#define USBD_MANUFACTURER_STRING "ODrive Robotics" +#define USBD_PID_FS 0x0D32 +#define USBD_PRODUCT_XSTR(s) USBD_PRODUCT_STR(s) +#define USBD_PRODUCT_STR(s) #s +#define USBD_PRODUCT_STRING_FS ODrive HW_VERSION_MAJOR.HW_VERSION_MINOR +#define USBD_SERIALNUMBER_STRING_FS "000000000001" +#define USBD_CONFIGURATION_STRING_FS "CDC Config" +#define USBD_INTERFACE_STRING_FS "CDC Interface" #define USB_SIZ_BOS_DESC 0x0C From 5d76fac8d5ec21ec445a738c3782b67a58c654e5 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 18 Jan 2018 00:35:05 -0800 Subject: [PATCH 125/155] Include 'version' in description --- Firmware/Src/usbd_desc.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/Src/usbd_desc.c b/Firmware/Src/usbd_desc.c index 380e06d6..82703697 100644 --- a/Firmware/Src/usbd_desc.c +++ b/Firmware/Src/usbd_desc.c @@ -77,7 +77,7 @@ #define USBD_PID_FS 0x0D32 #define USBD_PRODUCT_XSTR(s) USBD_PRODUCT_STR(s) #define USBD_PRODUCT_STR(s) #s -#define USBD_PRODUCT_STRING_FS ODrive HW_VERSION_MAJOR.HW_VERSION_MINOR +#define USBD_PRODUCT_STRING_FS ODrive version HW_VERSION_MAJOR.HW_VERSION_MINOR #define USBD_SERIALNUMBER_STRING_FS "000000000001" #define USBD_CONFIGURATION_STRING_FS "CDC Config" #define USBD_INTERFACE_STRING_FS "CDC Interface" From 1e1d883d75364dbe60792acd93e5aecc0eecbfb5 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 18 Jan 2018 00:36:54 -0800 Subject: [PATCH 126/155] update changelog --- Firmware/CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 92773f68..4bc92ef8 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -2,9 +2,11 @@ ### Added * UUID Endpoint +* Reporting of correct ODrive version on USB descriptor * Getting started instructions for VSCode ### Changed +* USB Product ID to 0x0D32, as it is the only Pid we were allocated on [pid.codes](http://pid.codes/1209/0D32/) * Recommended method to debug firmware from VSCode now uses Cortex-Debug extension instead of native-debug. * Refactor IDE instructions into separate files From 36e739be60c76ff1453fe9496c9f1735fbce74bd Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 18 Jan 2018 00:48:07 -0800 Subject: [PATCH 127/155] Update CHANGELOG.md --- Firmware/CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 4bc92ef8..566e8dc5 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,4 +1,4 @@ -## UNRELEASED +## [0.3.1] - 2018-01-18 ### Added * UUID Endpoint From ec054a1216151ba60a43424122ed03ae674058b9 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 22 Jan 2018 14:28:46 -0800 Subject: [PATCH 128/155] Change to rising edge --- Firmware/Inc/main.h | 2 +- Firmware/MotorControl/low_level.c | 6 +++--- Firmware/Src/gpio.c | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index a9650093..8ba31542 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -54,7 +54,7 @@ #define HW_VERSION_MAJOR 3 #define HW_VERSION_MINOR 4 -// #define HW_VERSION_HIGH_VOLTAGE true +#define HW_VERSION_HIGH_VOLTAGE true #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index dfba259a..1d4f43ee 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -34,7 +34,7 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (600 * 4) +#define ENCODER_CPR (2048 * 4) #define ENC_USE_INDEX_PIN true #define POLE_PAIRS 7 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); @@ -60,7 +60,7 @@ Motor_t motors[] = { .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, // .vel_setpoint = 800.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] @@ -159,7 +159,7 @@ Motor_t motors[] = { .pos_setpoint = 0.0f, .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] .vel_integrator_current = 0.0f, // [A] .vel_limit = 20000.0f, // [counts/s] diff --git a/Firmware/Src/gpio.c b/Firmware/Src/gpio.c index 7983667b..369c2c2e 100644 --- a/Firmware/Src/gpio.c +++ b/Firmware/Src/gpio.c @@ -200,7 +200,7 @@ void SetupENCIndexGPIO(){ /*Configure GPIO pins : PAPin PAPin */ GPIO_InitStruct.Pin = M0_ENC_Z_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); @@ -209,7 +209,7 @@ void SetupENCIndexGPIO(){ /*Configure GPIO pins : PBPin PBPin */ GPIO_InitStruct.Pin = M1_ENC_Z_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); From 6481e00d0e49837140d9b89973ea1c88d0834a09 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 24 Jun 2017 18:33:36 -0700 Subject: [PATCH 129/155] move out FOC voltage out of loop --- Firmware/MotorControl/low_level.c | 29 ++++++++++++++++------------- Firmware/MotorControl/low_level.h | 1 + 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 3de25b22..d109aea0 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -877,19 +877,7 @@ __attribute__((unused)) void FOC_voltage_loop(Motor_t* motor, float v_d, float v osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); update_rotor(motor); - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float v_alpha = c * v_d - s * v_q; - float v_beta = c * v_q + s * v_d; - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_FOC_VOLTAGE_TIMING; - return; - } + FOC_voltage(motor, v_d, v_q); } } @@ -1155,6 +1143,21 @@ void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { queue_modulation_timings(motor, mod_alpha, mod_beta); } +void FOC_voltage(Motor_t* motor, float v_d, float v_q) { + float phase = get_rotor_phase(motor); + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float v_alpha = c*v_d - s*v_q; + float v_beta = c*v_q + s*v_d; + queue_voltage_timings(motor, v_alpha, v_beta); + + // Check we meet deadlines after queueing + if (!(check_timing(motor) < motor->control_deadline)) { + motor->error = ERROR_FOC_VOLTAGE_TIMING; + return; + } +} + bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { Current_control_t* ictrl = &motor->current_control; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 22a09877..6589e91e 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -237,6 +237,7 @@ void update_brake_current(); void set_brake_current(float brake_current); void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta); void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta); +void FOC_voltage(Motor_t* motor, float v_d, float v_q); bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); void control_motor_loop(Motor_t* motor); From 35944a57504b5b377cadab9afe10a433ebc47ab1 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 24 Jun 2017 19:14:00 -0700 Subject: [PATCH 130/155] add motor types, including gimbal --- Firmware/MotorControl/low_level.c | 7 +++++-- Firmware/MotorControl/low_level.h | 7 +++++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index d109aea0..9f20227c 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -91,6 +91,8 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup + .motor_type = MOTOR_TYPE_HIGH_CURRENT, + // .motor_type = MOTOR_TYPE_GIMBAL, .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { @@ -187,8 +189,9 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .motor_type = MOTOR_TYPE_HIGH_CURRENT, + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // Read out max_allowed_current to see max supported value for current_lim. // You can change DRV8301_ShuntAmpGain to get a different range. diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 6589e91e..697d0a84 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -60,6 +60,12 @@ typedef enum { CTRL_MODE_POSITION_CONTROL } Motor_control_mode_t; +typedef enum { + MOTOR_TYPE_HIGH_CURRENT, + // MOTOR_TYPE_LOW_CURRENT, //Not yet implemented + MOTOR_TYPE_GIMBAL +} Motor_type_t; + typedef struct { float phB; float phC; @@ -149,6 +155,7 @@ typedef struct { Iph_BC_t DC_calib; DRV8301_Obj gate_driver; DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers + Motor_type_t motor_type; float shunt_conductance; float phase_current_rev_gain; //Reverse gain for ADC to Amps Current_control_t current_control; From 2ef82d1edb62e60be3ce9a1044782947eb93bbda Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 24 Jun 2017 19:50:26 -0700 Subject: [PATCH 131/155] implement gimbal motor mode --- Firmware/MotorControl/low_level.c | 40 +++++++++++++++++++++++-------- Firmware/MotorControl/low_level.h | 3 ++- 2 files changed, 32 insertions(+), 11 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 9f20227c..232f0b03 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -48,6 +48,9 @@ const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CP // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public + +// NOTE: for gimbal motors, all units that are A are instead V. +// example: vel_gain is [V/(count/s)] instead. Motor_t motors[] = { { // M0 @@ -786,14 +789,20 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { bool motor_calibration(Motor_t* motor) { motor->error = ERROR_NO_ERROR; - // #warning(hardcoded values for SK3-5065-280kv!) - // float R = 0.0332548246f; - // float L = 7.97315806e-06f; + float calibration_voltage = 0.0f; + if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) + return false; + calibration_voltage = motor->calibration_current * motor->phase_resistance; - if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) - return false; - if (!measure_phase_inductance(motor, -1.0f, 1.0f)) + if (!measure_phase_inductance(motor, -1.0f, 1.0f)) + return false; + } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { + calibration_voltage = motor->calibration_current; + } else { return false; + } + if (motor->rotor_mode == ROTOR_MODE_ENCODER || motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) @@ -1146,7 +1155,7 @@ void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { queue_modulation_timings(motor, mod_alpha, mod_beta); } -void FOC_voltage(Motor_t* motor, float v_d, float v_q) { +bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); float s = arm_sin_f32(phase); @@ -1157,8 +1166,9 @@ void FOC_voltage(Motor_t* motor, float v_d, float v_q) { // Check we meet deadlines after queueing if (!(check_timing(motor) < motor->control_deadline)) { motor->error = ERROR_FOC_VOLTAGE_TIMING; - return; + return false; } + return true; } bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { @@ -1314,8 +1324,18 @@ void control_motor_loop(Motor_t* motor) { motor->current_control.Iq = Iq; // Execute current command - if (!FOC_current(motor, 0.0f, Iq)) { - break; // in case of error exit loop, motor->error has been set by FOC_current + if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if(!FOC_current(motor, 0.0f, Iq)){ + break; // in case of error exit loop, motor->error has been set by FOC_current + } + } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { + //In gimbal motor mode, current is reinterptreted as voltage. + if(!FOC_voltage(motor, 0.0f, Iq)){ + break; // in case of error exit loop, motor->error has been set by FOC_voltage + } + } else { + motor->error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + break; } update_brake_current(); diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 697d0a84..54fcb01e 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -49,6 +49,7 @@ typedef enum { ERROR_POS_CTRL_DURING_SENSORLESS, ERROR_SPIN_UP_TIMEOUT, ERROR_DRV_FAULT, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, } Error_t; // Note: these should be sorted from lowest level of control to @@ -244,7 +245,7 @@ void update_brake_current(); void set_brake_current(float brake_current); void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta); void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta); -void FOC_voltage(Motor_t* motor, float v_d, float v_q); +bool FOC_voltage(Motor_t* motor, float v_d, float v_q); bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); void control_motor_loop(Motor_t* motor); From 6271a008d89d3f84e1dc891d95d60cc8da76e359 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 24 Jun 2017 20:40:52 -0700 Subject: [PATCH 132/155] make gimbal note a bit easier to understand --- Firmware/MotorControl/low_level.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 232f0b03..4ff11ed1 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -49,8 +49,8 @@ const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CP // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public -// NOTE: for gimbal motors, all units that are A are instead V. -// example: vel_gain is [V/(count/s)] instead. +// NOTE: for gimbal motors, all units of A are instead V. +// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] Motor_t motors[] = { { // M0 @@ -193,8 +193,8 @@ Motor_t motors[] = { }, // .gate_driver_regs Init by DRV8301_setup .motor_type = MOTOR_TYPE_HIGH_CURRENT, - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // Read out max_allowed_current to see max supported value for current_lim. // You can change DRV8301_ShuntAmpGain to get a different range. From f6b36345a2df70a3e84a763aed0652abe6096c87 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 22 Jan 2018 16:53:50 -0800 Subject: [PATCH 133/155] fix voltage on enc calib --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 4ff11ed1..b44751fb 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -805,7 +805,7 @@ bool motor_calibration(Motor_t* motor) { if (motor->rotor_mode == ROTOR_MODE_ENCODER || motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) + if (!calib_enc_offset(motor, calibration_voltage)) return false; } From 44495601d5340ab2fe4beb7120d25cdba90696a0 Mon Sep 17 00:00:00 2001 From: Brandon Kinman Date: Mon, 22 Jan 2018 21:41:42 -0800 Subject: [PATCH 134/155] Update README.md Added short section about Gimbal mode selection. --- Firmware/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Firmware/README.md b/Firmware/README.md index a72a6eb6..8d663017 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -70,6 +70,15 @@ You must set: * `ENCODER_CPR`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. * `POLE_PAIRS`: This is the number of magnet poles in the rotor, divided by two. You can simply count the number of magnets in the rotor, if you can see them. * `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f. +* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (MOTOR_TYPE_GIMBAL). + +### Motor Mode +The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. + +If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. +If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. + +If 100's of mA current noise is "large" for you, and you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive. ### Tuning parameters The most important parameters are the limits: From 8fb461dddd10b21096ef6fde7aea9cbc43545227 Mon Sep 17 00:00:00 2001 From: Brandon Kinman Date: Mon, 22 Jan 2018 21:57:50 -0800 Subject: [PATCH 135/155] Update README.md Made wording simpler for newbies. --- Firmware/README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Firmware/README.md b/Firmware/README.md index 8d663017..b1a29481 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -72,8 +72,10 @@ You must set: * `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f. * `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (MOTOR_TYPE_GIMBAL). -### Motor Mode -The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. +### Motor Modes +The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For high-torque gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. + +**Further detail:** If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. From cf1f60a62af1157b62fb3c7edd90e2f836fe3575 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 22 Jan 2018 22:00:27 -0800 Subject: [PATCH 136/155] Update README.md --- Firmware/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index b1a29481..0d484e41 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -70,7 +70,7 @@ You must set: * `ENCODER_CPR`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. * `POLE_PAIRS`: This is the number of magnet poles in the rotor, divided by two. You can simply count the number of magnets in the rotor, if you can see them. * `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f. -* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (MOTOR_TYPE_GIMBAL). +* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). ### Motor Modes The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For high-torque gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. From 248a0882662fc70a02ddddaf088f23801d9f3133 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 22 Jan 2018 22:00:43 -0800 Subject: [PATCH 137/155] Update README.md --- Firmware/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/README.md b/Firmware/README.md index 0d484e41..07b88e66 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -70,7 +70,7 @@ You must set: * `ENCODER_CPR`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. * `POLE_PAIRS`: This is the number of magnet poles in the rotor, divided by two. You can simply count the number of magnets in the rotor, if you can see them. * `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f. -* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (` MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). +* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). ### Motor Modes The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For high-torque gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. From 7311cf1f42f761ea176975d3d82c3a4ebc9a8983 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 22 Jan 2018 22:02:54 -0800 Subject: [PATCH 138/155] Update low_level.c --- Firmware/MotorControl/low_level.c | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index b44751fb..f945d2df 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -51,6 +51,7 @@ const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CP // NOTE: for gimbal motors, all units of A are instead V. // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] +// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. Motor_t motors[] = { { // M0 From 236b73a8b40aabc53abf3b508d2dce740babf095 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 23 Jan 2018 20:31:09 -0800 Subject: [PATCH 139/155] Update CHANGELOG.md --- Firmware/CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 566e8dc5..5150d704 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,3 +1,8 @@ +## UNRELEASED + +### Added +* Gimbal motor mode + ## [0.3.1] - 2018-01-18 ### Added From 8ab73b0baa16be65ce91c5e0d9c23375e137105c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 26 Jan 2018 17:51:32 -0800 Subject: [PATCH 140/155] turn off control at start for enc idx testing --- Firmware/MotorControl/axis.h | 4 ++-- Firmware/MotorControl/low_level.c | 1 + Firmware/Src/gpio.c | 3 +++ 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/axis.h b/Firmware/MotorControl/axis.h index b3cdf543..8be98a22 100644 --- a/Firmware/MotorControl/axis.h +++ b/Firmware/MotorControl/axis.h @@ -12,8 +12,8 @@ extern "C" { // TODO: decide if we want to consolidate all default configs in one file for ease of use? struct AxisConfig { - bool enable_control_at_start = true; - bool do_calibration_at_start = true; + bool enable_control_at_start = false; + bool do_calibration_at_start = false; }; class Axis { diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 1d4f43ee..73fea597 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -537,6 +537,7 @@ void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { setEncoderCount(&motors[motor_index], 0); motors[motor_index].encoder.index_found = true; } + //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default if(GPIO_Pin == M0_ENC_Z_Pin){ HAL_NVIC_DisableIRQ(EXTI3_IRQn); } else { diff --git a/Firmware/Src/gpio.c b/Firmware/Src/gpio.c index 369c2c2e..98876c3a 100644 --- a/Firmware/Src/gpio.c +++ b/Firmware/Src/gpio.c @@ -191,6 +191,7 @@ void SetGPIO12toStepDir() { GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct); + //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0); HAL_NVIC_EnableIRQ(EXTI0_IRQn); } @@ -204,6 +205,7 @@ void SetupENCIndexGPIO(){ GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0); HAL_NVIC_EnableIRQ(EXTI15_10_IRQn); @@ -213,6 +215,7 @@ void SetupENCIndexGPIO(){ GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0); HAL_NVIC_EnableIRQ(EXTI3_IRQn); } From 13deca539401c248db981b966c4cf796bbc2d876 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 26 Jan 2018 19:15:48 -0800 Subject: [PATCH 141/155] Keep rotor estimation up to date while idling --- Firmware/MotorControl/axis.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index f94ad87f..f50813a4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -60,6 +60,10 @@ void Axis::StateMachineLoop() { legacy_motor_ref_->thread_ready = true; bool calibration_ok = false; for (;;) { + // Keep rotor estimation up to date while idling + osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); + update_rotor(legacy_motor_ref_); + if (do_calibration_) { do_calibration_ = false; @@ -86,9 +90,6 @@ void Axis::StateMachineLoop() { enable_control_ = false; } } - - // give some time to lower priority threads - osDelay(2); } legacy_motor_ref_->thread_ready = false; } \ No newline at end of file From 4d7c3b92e30650bdf698a6586f7dc24a2353d539 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 26 Jan 2018 20:18:41 -0800 Subject: [PATCH 142/155] fix setEncoderCount wrong TIM bug --- Firmware/.vscode/c_cpp_properties.json | 10 +++++----- Firmware/MotorControl/low_level.c | 13 ++++++++----- Firmware/Src/gpio.c | 4 ++-- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index f732a2df..691314ac 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -16,16 +16,16 @@ "${workspaceRoot}/Drivers/CMSIS/Include", "${workspaceRoot}/Inc", "${workspaceRoot}/MotorControl", - "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include", "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1", "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1/arm-none-eabi", - "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include" + "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include", + "C:/Program Files (x86)/GNU Tools ARM Embedded/6 2017-q1-update/arm-none-eabi/include" ], "defines": [ "_DEBUG", "UNICODE" ], - "intelliSenseMode": "msvc-x64", + "intelliSenseMode": "clang-x64", "browse": { "path": [ "${workspaceRoot}", @@ -69,7 +69,7 @@ "_DEBUG", "UNICODE" ], - "intelliSenseMode": "msvc-x64", + "intelliSenseMode": "clang-x64", "browse": { "path": [ "${workspaceRoot}", @@ -115,7 +115,7 @@ "_DEBUG", "UNICODE" ], - "intelliSenseMode": "msvc-x64", + "intelliSenseMode": "clang-x64", "browse": { "path": [ "${workspaceRoot}", diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 73fea597..55373716 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -350,7 +350,9 @@ void init_motor_control() { // Start Encoders HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); - SetupENCIndexGPIO(); + if (ENC_USE_INDEX_PIN) { + SetupENCIndexGPIO(); + } // Wait for current sense calibration to converge // TODO make timing a function of calibration filter tau @@ -533,9 +535,10 @@ void step_cb(uint16_t GPIO_Pin) { // Triggered when an encoder passes over the "Index" pin void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { - if (!motors[motor_index].encoder.index_found) { - setEncoderCount(&motors[motor_index], 0); - motors[motor_index].encoder.index_found = true; + Motor_t* motor = &motors[motor_index]; + if (!motor->encoder.index_found) { + setEncoderCount(motor, 0); + motor->encoder.index_found = true; } //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default if(GPIO_Pin == M0_ENC_Z_Pin){ @@ -1072,7 +1075,7 @@ void setEncoderCount(Motor_t* motor, uint32_t count) { uint32_t prim = __get_PRIMASK(); __disable_irq(); motor->encoder.encoder_state = count; - motor->motor_timer->Instance->CNT = count; + motor->encoder.encoder_timer->Instance->CNT = count; motor->encoder.pll_pos = (float)count; __set_PRIMASK(prim); } diff --git a/Firmware/Src/gpio.c b/Firmware/Src/gpio.c index 98876c3a..7893e703 100644 --- a/Firmware/Src/gpio.c +++ b/Firmware/Src/gpio.c @@ -203,7 +203,7 @@ void SetupENCIndexGPIO(){ GPIO_InitStruct.Pin = M0_ENC_Z_Pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + HAL_GPIO_Init(M0_ENC_Z_GPIO_Port, &GPIO_InitStruct); //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0); @@ -213,7 +213,7 @@ void SetupENCIndexGPIO(){ GPIO_InitStruct.Pin = M1_ENC_Z_Pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + HAL_GPIO_Init(M1_ENC_Z_GPIO_Port, &GPIO_InitStruct); //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0); From 9b7d201276b0d735e88351fbe88cb7bf30115553 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 26 Jan 2018 21:02:58 -0800 Subject: [PATCH 143/155] manual idx search on M1 working, not on M0 --- Firmware/MotorControl/axis.cpp | 4 ++++ Firmware/MotorControl/axis.h | 4 ++-- Firmware/MotorControl/low_level.c | 10 ++++++---- Firmware/MotorControl/low_level.h | 1 + Firmware/Src/gpio.c | 1 + 5 files changed, 14 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index f50813a4..4c128424 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -64,6 +64,10 @@ void Axis::StateMachineLoop() { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); update_rotor(legacy_motor_ref_); + // TODO: actual scan search + if (legacy_motor_ref_->encoder.use_index && !legacy_motor_ref_->encoder.index_found) + continue; //Keep waiting for manual index locating + if (do_calibration_) { do_calibration_ = false; diff --git a/Firmware/MotorControl/axis.h b/Firmware/MotorControl/axis.h index 8be98a22..b3cdf543 100644 --- a/Firmware/MotorControl/axis.h +++ b/Firmware/MotorControl/axis.h @@ -12,8 +12,8 @@ extern "C" { // TODO: decide if we want to consolidate all default configs in one file for ease of use? struct AxisConfig { - bool enable_control_at_start = false; - bool do_calibration_at_start = false; + bool enable_control_at_start = true; + bool do_calibration_at_start = true; }; class Axis { diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 89caf961..fc825188 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -35,7 +35,6 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct #define ENCODER_CPR (2048 * 4) -#define ENC_USE_INDEX_PIN true #define POLE_PAIRS 7 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); @@ -120,7 +119,8 @@ Motor_t motors[] = { .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { .encoder_timer = &htim3, - .index_found = !(ENC_USE_INDEX_PIN), + .use_index = true, + .index_found = false, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, @@ -216,7 +216,8 @@ Motor_t motors[] = { .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { .encoder_timer = &htim4, - .index_found = !(ENC_USE_INDEX_PIN), + .use_index = true, + .index_found = false, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, @@ -357,7 +358,8 @@ void init_motor_control() { // Start Encoders HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); - if (ENC_USE_INDEX_PIN) { + //TODO: Enable index on only one channel + if (motors[0].encoder.use_index || motors[1].encoder.use_index) { SetupENCIndexGPIO(); } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 34d88c8d..bd21a5da 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -110,6 +110,7 @@ typedef struct { typedef struct { TIM_HandleTypeDef* encoder_timer; + bool use_index; bool index_found; int encoder_cpr; int32_t encoder_offset; diff --git a/Firmware/Src/gpio.c b/Firmware/Src/gpio.c index 7893e703..693ab7b1 100644 --- a/Firmware/Src/gpio.c +++ b/Firmware/Src/gpio.c @@ -196,6 +196,7 @@ void SetGPIO12toStepDir() { HAL_NVIC_EnableIRQ(EXTI0_IRQn); } +//TODO: Enable index on only one channel void SetupENCIndexGPIO(){ GPIO_InitTypeDef GPIO_InitStruct; From 46b0ddc002d78ef23905aa4f8034ff406fd24699 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 26 Jan 2018 21:13:44 -0800 Subject: [PATCH 144/155] manual idx search working on both motors --- Firmware/MotorControl/low_level.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index fc825188..3169f436 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -551,9 +551,9 @@ void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { } //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default if(GPIO_Pin == M0_ENC_Z_Pin){ - HAL_NVIC_DisableIRQ(EXTI3_IRQn); - } else { HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); + } else { + HAL_NVIC_DisableIRQ(EXTI3_IRQn); } } From 1f8bba475e381b13c68f30e3b64859adc595123f Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 26 Jan 2018 22:10:16 -0800 Subject: [PATCH 145/155] index searching works --- Firmware/MotorControl/axis.cpp | 6 +----- Firmware/MotorControl/low_level.c | 17 +++++++++++++---- Firmware/MotorControl/low_level.h | 3 ++- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 4c128424..aaac53b5 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -63,11 +63,7 @@ void Axis::StateMachineLoop() { // Keep rotor estimation up to date while idling osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); update_rotor(legacy_motor_ref_); - - // TODO: actual scan search - if (legacy_motor_ref_->encoder.use_index && !legacy_motor_ref_->encoder.index_found) - continue; //Keep waiting for manual index locating - + if (do_calibration_) { do_calibration_ = false; diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 3169f436..3764fbb7 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -121,6 +121,7 @@ Motor_t motors[] = { .encoder_timer = &htim3, .use_index = true, .index_found = false, + .calibrated = false, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, @@ -218,6 +219,7 @@ Motor_t motors[] = { .encoder_timer = &htim4, .use_index = true, .index_found = false, + .calibrated = false, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, @@ -758,7 +760,7 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; int32_t encvaluesum = 0; - // go to encoder zero phase for start_lock_duration to get ready to scan + // go to motor zero phase for start_lock_duration to get ready to scan for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; @@ -828,7 +830,10 @@ bool motor_calibration(Motor_t* motor) { } if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { + motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { + if (motor->encoder.use_index && !motor->encoder.index_found) + if (!scan_for_enc_idx(motor, 10.0f, calibration_voltage)) + return false; if (!calib_enc_offset(motor, calibration_voltage)) return false; } @@ -889,10 +894,14 @@ bool anti_cogging_calibration(Motor_t* motor) { // Test functions //-------------------------------- -__attribute__((unused)) void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { +bool scan_for_enc_idx(Motor_t* motor, float omega, float voltage_magnitude) { for (;;) { for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); + + if (motor->encoder.index_found) + return true; + float v_alpha = voltage_magnitude * arm_cos_f32(ph); float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); @@ -901,7 +910,7 @@ __attribute__((unused)) void scan_motor_loop(Motor_t* motor, float omega, float motor->last_cpu_time = check_timing(motor); if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_SCAN_MOTOR_TIMING; - return; + return false; } } } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index bd21a5da..07a79688 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -112,6 +112,7 @@ typedef struct { TIM_HandleTypeDef* encoder_timer; bool use_index; bool index_found; + bool calibrated; int encoder_cpr; int32_t encoder_offset; int32_t encoder_state; @@ -232,11 +233,11 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage); bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high); bool calib_enc_offset(Motor_t* motor, float voltage_magnitude); +bool scan_for_enc_idx(Motor_t* motor, float v_d, float v_q); bool anti_cogging_calibration(Motor_t* motor); // Test functions void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude); -void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q); // Main motor control void update_rotor(Motor_t* motor); bool using_encoder(Motor_t* motor); From aaa5f802de7625090e5f70642a37f4978443ec1d Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 26 Jan 2018 22:34:34 -0800 Subject: [PATCH 146/155] configurable search speed and dir, check for already flashed calibration --- Firmware/MotorControl/low_level.c | 16 +++++++++++----- Firmware/MotorControl/low_level.h | 1 + 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 3764fbb7..48fb730e 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -122,10 +122,11 @@ Motor_t motors[] = { .use_index = true, .index_found = false, .calibrated = false, + .idx_search_speed = 10.0f, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset + .motor_dir = 1, // 1 or -1 .phase = 0.0f, // [rad] .pll_pos = 0.0f, // [rad] .pll_vel = 0.0f, // [rad/s] @@ -220,10 +221,11 @@ Motor_t motors[] = { .use_index = true, .index_found = false, .calibrated = false, + .idx_search_speed = 10.0f, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset + .motor_dir = 1, // 1 or -1 .phase = 0.0f, // [rad] .pll_pos = 0.0f, // [rad] .pll_vel = 0.0f, // [rad/s] @@ -809,6 +811,7 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { int offset = encvaluesum / (num_steps * 2); motor->encoder.encoder_offset = offset; + motor->encoder.calibrated = true; return true; } @@ -832,10 +835,13 @@ bool motor_calibration(Motor_t* motor) { if (motor->rotor_mode == ROTOR_MODE_ENCODER || motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { if (motor->encoder.use_index && !motor->encoder.index_found) - if (!scan_for_enc_idx(motor, 10.0f, calibration_voltage)) + if (!scan_for_enc_idx(motor, + (float)(motor->encoder.motor_dir) * motor->encoder.idx_search_speed, + calibration_voltage)) + return false; + if (!motor->encoder.calibrated) + if (!calib_enc_offset(motor, calibration_voltage)) return false; - if (!calib_enc_offset(motor, calibration_voltage)) - return false; } // Calculate current control gains diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 07a79688..194e0a49 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -113,6 +113,7 @@ typedef struct { bool use_index; bool index_found; bool calibrated; + float idx_search_speed; int encoder_cpr; int32_t encoder_offset; int32_t encoder_state; From a7c8a85d30e43cb5bf1f14a27ae4b60a2ee658e9 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 26 Jan 2018 22:59:21 -0800 Subject: [PATCH 147/155] expose motor_dir --- Firmware/MotorControl/commands.cpp | 6 ++++-- Firmware/MotorControl/low_level.h | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index ed9a263c..414d03d3 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -137,6 +137,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("pll_ki", &motors[0].encoder.pll_ki), Endpoint::make_property("encoder_offset", &motors[0].encoder.encoder_offset), Endpoint::make_property("encoder_state", &motors[0].encoder.encoder_state), + Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir), Endpoint::close_tree(), Endpoint::make_function("set_pos_setpoint", &motors_0_set_pos_setpoint_func), Endpoint::make_property("pos_setpoint", &motors[0].set_pos_setpoint_args.pos_setpoint), @@ -189,8 +190,9 @@ const Endpoint endpoints[] = { Endpoint::make_property("pll_vel", &motors[1].encoder.pll_vel), Endpoint::make_property("pll_kp", &motors[1].encoder.pll_kp), Endpoint::make_property("pll_ki", &motors[1].encoder.pll_ki), - Endpoint::make_property("encoder_offset", reinterpret_cast(&motors[1].encoder.encoder_offset)), - Endpoint::make_property("encoder_state", reinterpret_cast(&motors[1].encoder.encoder_state)), + Endpoint::make_property("encoder_offset", &motors[1].encoder.encoder_offset), + Endpoint::make_property("encoder_state", &motors[1].encoder.encoder_state), + Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir), Endpoint::close_tree(), Endpoint::make_function("set_pos_setpoint", &motors_1_set_pos_setpoint_func), Endpoint::make_property("pos_setpoint", &motors[1].set_pos_setpoint_args.pos_setpoint), diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 194e0a49..6ff91fd4 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -117,7 +117,7 @@ typedef struct { int encoder_cpr; int32_t encoder_offset; int32_t encoder_state; - int motor_dir; // 1/-1 for fwd/rev alignment to encoder. + int32_t motor_dir; // 1/-1 for fwd/rev alignment to encoder. float phase; float pll_pos; float pll_vel; From 91f72a60cdb5c9ba5d21ae477f7be04769b9a5b5 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 27 Jan 2018 00:16:49 -0800 Subject: [PATCH 148/155] write up instructions --- Firmware/MotorControl/low_level.c | 4 ++-- Firmware/README.md | 25 +++++++++++++++++++++++++ README.md | 2 ++ 3 files changed, 29 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 48fb730e..f750c849 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -122,7 +122,7 @@ Motor_t motors[] = { .use_index = true, .index_found = false, .calibrated = false, - .idx_search_speed = 10.0f, + .idx_search_speed = 10.0f, // [rad/s electrical] .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, @@ -221,7 +221,7 @@ Motor_t motors[] = { .use_index = true, .index_found = false, .calibrated = false, - .idx_search_speed = 10.0f, + .idx_search_speed = 10.0f, // [rad/s electrical] .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, diff --git a/Firmware/README.md b/Firmware/README.md index 07b88e66..f9ca56f1 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -15,6 +15,7 @@ The project is under active development, so make sure to check the [Changelog](C - [Setting up an IDE](#setting-up-an-ide) - [Continuing without an IDE](#no-ide-instructions) - [Communicating over USB or UART](#communicating-over-usb-or-uart) +- [Encoder Calibration](#encoder-calibration) - [Generating startup code](#generating-startup-code) - [Notes for Contributors](#notes-for-contributors) @@ -87,6 +88,7 @@ The most important parameters are the limits: * The current limit: `.current_lim = 75.0f, //[A] // Note: consistent with 40v/v gain`. The default current limit, for safety reasons, is set to 10A. This is quite weak, and good for making sure the drive is stable. Once you have tuned the drive, you can increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains. * Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current. * The velocity limit: `.vel_limit = 20000.0f, // [counts/s]`. The motor will be limited to this speed; again the default value is quite slow. +* You can change `.calibration_current` to the largest value you feel comfortable leaving running through the motor continously when the motor is stationary. The motion control gains are currently manually tuned: * `.pos_gain = 20.0f, // [(counts/s) / counts]` @@ -201,6 +203,29 @@ pip install pyusb pyserial ### Other platforms See the [protocol specification](protocol.md) or the [legacy protocol specification](legacy-protocol.md). +

+## Encoder Calibration +By default the encoder-to-motor calibration will run on every startup. During encoder calibration the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. + +### Encoder with Index signal +If you have an encoder with an index (Z) signal, you may avoid having to do the calibration on every startup, and instead use the index signal to re-sync the encoder to a stored calibration. Bleow are the steps to do the one-time calibration and configuration. Note that you can follow these steps with one motor at a time, or all motors together, as you wish. + +* Since you will only do this once, it is recommended that you mechanically disengage the motor from anything other than the encoder, so it can spin freely. +* All the parameters we will be modifying are in the motor structs at the top of [MotorControl/low_level.c](MotorControl/low_level.c). +* Set `.encoder.use_index = true` and `.encoder.calibrated = false`. +* Flash this configuration, and let the motor scan for the index pulse and then complete the encoder calibration. +* Run `explore_odrive.py`, check [Communicating over USB or UART](#communicating-over-usb-or-uart) for instructions on how to do that. +* Enter the following to print out the calibration parameters (substitute the motor number you are calibrating for ``): + * `my_odrive.motor.encoder.encoder_offset` - This should print a number, like -326 or 1364. + * `my_odrive.motor.encoder.motor_dir` - This should print 1 or -1. +* Copy these numbers to the corresponding entries in low_level.c: `.encoder.encoder_offset` and `.encoder.motor_dir`. + * _Warning_: Please be careful to enter the correct numbers, and not to confuse the motor channels. Incorrect values may cause the motor to spin out of control. +* Set `.encoder.calibrated = true`. +* Flash this configuration and check that the motor scans for the index pulse but skips the encoder calibration. +* Congratulations, you are now done. You may now attach the motor to your mechanical load. +* If you wish to scan for the index pulse in the other direction (if for example your axis usually starts close to a hard-stop), you can set a negative value in `.encoder.idx_search_speed`. +* If your motor has problems reaching the index location due to the mechanical load, you can increase `.calibration_current`. +

## Generating startup code **Note:** You do not need to run this step to program the board. This is only required if you wish to update the auto generated code. diff --git a/README.md b/README.md index dacfbb6c..e2b4349a 100644 --- a/README.md +++ b/README.md @@ -33,6 +33,8 @@ The startup procedure is demonstrated [here](https://www.youtube.com/watch?v=VCX Note: the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. Also note that in the video, the motors spin after initalisation, but in the current software the default behaviour is to do position control to position 0 (i.e. the position at startup) +If you have an encoder with an index (Z) signal, you can calibrate once and restore the calibration on startup. Instructions on how to do that are [here](Firmware/README.md#encoder-calibration). + ### Sending commands Sending USB and UART commands is documented [here](Firmware/README.md#communicating-over-usb-and-uart). You can also have a look at the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) that makes it easy to use the UART interface on Arduino. You can also look at it as an implementation example of how to talk to the ODrive over UART. From ad38d12a0e7d9b30686a78f657821a7d73b30848 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 27 Jan 2018 00:19:05 -0800 Subject: [PATCH 149/155] default to not HV version --- Firmware/Inc/main.h | 2 +- Firmware/MotorControl/low_level.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/Inc/main.h b/Firmware/Inc/main.h index 8ba31542..a9650093 100644 --- a/Firmware/Inc/main.h +++ b/Firmware/Inc/main.h @@ -54,7 +54,7 @@ #define HW_VERSION_MAJOR 3 #define HW_VERSION_MINOR 4 -#define HW_VERSION_HIGH_VOLTAGE true +// #define HW_VERSION_HIGH_VOLTAGE true #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index f750c849..31035930 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -34,7 +34,7 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (2048 * 4) +#define ENCODER_CPR (2048 * 4) // Default resolution of CUI-AMT102 encoder #define POLE_PAIRS 7 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); From de86acf0b099ed035cf71da607a8660599c3a84c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 30 Jan 2018 09:15:28 -0800 Subject: [PATCH 150/155] Update CHANGELOG.md --- Firmware/CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 5150d704..1dc98bac 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -2,6 +2,7 @@ ### Added * Gimbal motor mode +* Encoder index pulse support ## [0.3.1] - 2018-01-18 From 5f3341b949e12bf8b185a45bd9a8da07acf38dbe Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 2 Feb 2018 22:45:19 -0800 Subject: [PATCH 151/155] add resistance_calib_max_voltage parameter --- Firmware/CHANGELOG.md | 3 ++- Firmware/MotorControl/low_level.c | 17 ++++++++++------- Firmware/MotorControl/low_level.h | 1 + 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 1dc98bac..48bacd7a 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,8 +1,9 @@ -## UNRELEASED +## [0.3.2] - 2018-02-02 ### Added * Gimbal motor mode * Encoder index pulse support +* `resistance_calib_max_voltage` parameter ## [0.3.1] - 2018-01-18 diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 31035930..f7527821 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -71,6 +71,7 @@ Motor_t motors[] = { .vel_limit = 20000.0f, // [counts/s] .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] + .resistance_calib_max_voltage = 1.0f, // [V] .phase_inductance = 0.0f, // to be set by measure_phase_inductance .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, @@ -173,6 +174,7 @@ Motor_t motors[] = { .vel_limit = 20000.0f, // [counts/s] .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] + .resistance_calib_max_voltage = 1.0f, // [V] .phase_inductance = 0.0f, // to be set by measure_phase_inductance .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, @@ -818,16 +820,17 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { bool motor_calibration(Motor_t* motor) { motor->error = ERROR_NO_ERROR; - float calibration_voltage = 0.0f; + float R_calib_max_voltage = motor->resistance_calib_max_voltage; + float enc_calibration_voltage = 0.0f; if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) + if (!measure_phase_resistance(motor, motor->calibration_current, R_calib_max_voltage)) return false; - calibration_voltage = motor->calibration_current * motor->phase_resistance; + enc_calibration_voltage = motor->calibration_current * motor->phase_resistance; - if (!measure_phase_inductance(motor, -1.0f, 1.0f)) + if (!measure_phase_inductance(motor, -R_calib_max_voltage, R_calib_max_voltage)) return false; } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { - calibration_voltage = motor->calibration_current; + enc_calibration_voltage = motor->calibration_current; } else { return false; } @@ -837,10 +840,10 @@ bool motor_calibration(Motor_t* motor) { if (motor->encoder.use_index && !motor->encoder.index_found) if (!scan_for_enc_idx(motor, (float)(motor->encoder.motor_dir) * motor->encoder.idx_search_speed, - calibration_voltage)) + enc_calibration_voltage)) return false; if (!motor->encoder.calibrated) - if (!calib_enc_offset(motor, calibration_voltage)) + if (!calib_enc_offset(motor, enc_calibration_voltage)) return false; } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 6ff91fd4..5da77803 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -145,6 +145,7 @@ typedef struct { float vel_limit; float current_setpoint; float calibration_current; + float resistance_calib_max_voltage; float phase_inductance; float phase_resistance; osThreadId motor_thread; From 6810e52d8decedcc275fb4358181051930caacfa Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 3 Feb 2018 19:09:49 -0800 Subject: [PATCH 152/155] expose Iq setpoint and measured separatly --- Firmware/MotorControl/commands.cpp | 6 ++++-- Firmware/MotorControl/low_level.c | 11 ++++++++--- Firmware/MotorControl/low_level.h | 3 ++- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 414d03d3..96cd86b1 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -126,7 +126,8 @@ const Endpoint endpoints[] = { Endpoint::make_property("i_gain", &motors[0].current_control.i_gain), Endpoint::make_property("v_current_control_integral_d", &motors[0].current_control.v_current_control_integral_d), Endpoint::make_property("v_current_control_integral_q", &motors[0].current_control.v_current_control_integral_q), - Endpoint::make_property("Iq_command", &motors[0].current_control.Iq), + Endpoint::make_property("Iq_setpoint", &motors[0].current_control.Iq_setpoint), + Endpoint::make_property("Iq_measured", &motors[0].current_control.Iq_measured), Endpoint::make_property("Ibus", const_cast(&motors[0].current_control.Ibus)), Endpoint::close_tree(), Endpoint::make_object("encoder"), @@ -181,7 +182,8 @@ const Endpoint endpoints[] = { Endpoint::make_property("i_gain", &motors[1].current_control.i_gain), Endpoint::make_property("v_current_control_integral_d", &motors[1].current_control.v_current_control_integral_d), Endpoint::make_property("v_current_control_integral_q", &motors[1].current_control.v_current_control_integral_q), - Endpoint::make_property("Iq_command", &motors[1].current_control.Iq), + Endpoint::make_property("Iq_setpoint", &motors[1].current_control.Iq_setpoint), + Endpoint::make_property("Iq_measured", &motors[1].current_control.Iq_measured), Endpoint::make_property("Ibus", const_cast(&motors[1].current_control.Ibus)), Endpoint::close_tree(), Endpoint::make_object("encoder"), diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index f7527821..b48bc931 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -112,7 +112,8 @@ Motor_t motors[] = { .Ibus = 0.0f, .final_v_alpha = 0.0f, .final_v_beta = 0.0f, - .Iq = 0.0f, + .Iq_setpoint = 0.0f, + .Iq_measured = 0.0f, .max_allowed_current = 0.0f, }, // .rotor_mode = ROTOR_MODE_SENSORLESS, @@ -214,7 +215,8 @@ Motor_t motors[] = { .Ibus = 0.0f, .final_v_alpha = 0.0f, .final_v_beta = 0.0f, - .Iq = 0.0f, + .Iq_setpoint = 0.0f, + .Iq_measured = 0.0f, .max_allowed_current = 0.0f, }, .rotor_mode = ROTOR_MODE_ENCODER, @@ -1216,6 +1218,9 @@ bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { Current_control_t* ictrl = &motor->current_control; + // For Reporting + ictrl->Iq_setpoint = Iq_des; + // Clarke transform float Ialpha = -motor->current_meas.phB - motor->current_meas.phC; float Ibeta = one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC); @@ -1226,6 +1231,7 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { float s = arm_sin_f32(phase); float Id = c * Ialpha + s * Ibeta; float Iq = c * Ibeta - s * Ialpha; + ictrl->Iq_measured = Iq; // Current error float Ierr_d = Id_des - Id; @@ -1364,7 +1370,6 @@ void control_motor_loop(Motor_t* motor) { } } - motor->current_control.Iq = Iq; // Execute current command if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { if(!FOC_current(motor, 0.0f, Iq)){ diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 5da77803..91d03c76 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -82,7 +82,8 @@ typedef struct { // Voltage applied at end of cycle: float final_v_alpha; // [V] float final_v_beta; // [V] - float Iq; + float Iq_setpoint; + float Iq_measured; float max_allowed_current; } Current_control_t; From 0f64f03d67c6cee46f7da23fd8b6aaceb823bb68 Mon Sep 17 00:00:00 2001 From: Brandon Kinman Date: Mon, 5 Feb 2018 20:31:41 -0800 Subject: [PATCH 153/155] Added compatibility for python 2.7 --- tools/demo.py | 2 ++ tools/odrive/core.py | 2 +- tools/odrive/protocol.py | 29 ++++++++++++++++++++++------- tools/odrive/usbbulk_transport.py | 4 +++- 4 files changed, 28 insertions(+), 9 deletions(-) diff --git a/tools/demo.py b/tools/demo.py index ab2fbe2a..4a06c260 100755 --- a/tools/demo.py +++ b/tools/demo.py @@ -3,6 +3,8 @@ Example usage of the ODrive python library to monitor and control ODrive devices """ +from __future__ import print_function + import odrive.core import time import math diff --git a/tools/odrive/core.py b/tools/odrive/core.py index c4888de7..bda8b58e 100644 --- a/tools/odrive/core.py +++ b/tools/odrive/core.py @@ -174,7 +174,7 @@ def create_object(name, json_data, namespace, channel, printer=noprint): attributes[member_name] = attribute # Create a type from the property list and instantiate it - jit_type = type(namespace, (object,), attributes) + jit_type = type(str(namespace), (object,), attributes) new_object = jit_type() return new_object diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index 1f8c9114..a8586667 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -2,7 +2,18 @@ import time import struct -from abc import ABC, abstractmethod +import sys + +import abc + +if sys.version_info >= (3, 4): + ABC = abc.ABC +else: + ABC = abc.ABCMeta('ABC', (), {}) + +if sys.version_info <= (3,4): + from monotonic import monotonic + time.monotonic = monotonic SYNC_BYTE = 0xAA CRC8_INIT = 0x42 @@ -30,6 +41,8 @@ def calc_crc(remainder, value, polynomial, bitwidth): def calc_crc8(remainder, value): if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list): for byte in value: + if not isinstance(byte,int): + byte = ord(byte) remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8) else: remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8) @@ -38,6 +51,8 @@ def calc_crc8(remainder, value): def calc_crc16(remainder, value): if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list): for byte in value: + if not isinstance(byte, int): + byte = ord(byte) remainder = calc_crc(remainder, byte, CRC16_DEFAULT, 16) else: remainder = calc_crc(remainder, value, CRC16_DEFAULT, 16) @@ -59,22 +74,22 @@ class DeviceInitException(Exception): class StreamSource(ABC): - @abstractmethod + @abc.abstractmethod def get_bytes(self, deadline): pass class StreamSink(ABC): - @abstractmethod + @abc.abstractmethod def process_bytes(self, bytes): pass class PacketSource(ABC): - @abstractmethod + @abc.abstractmethod def get_packet(self, deadline): pass class PacketSink(ABC): - @abstractmethod + @abc.abstractmethod def process_packet(self, packet): pass @@ -272,7 +287,7 @@ class Channel(PacketSink): self._expected_acks[seq_no] = packet[2:] else: - #if (calc_crc16(crc16, struct.pack(' Date: Wed, 7 Feb 2018 22:57:49 -0800 Subject: [PATCH 154/155] Set .use_index to false by default --- Firmware/MotorControl/low_level.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index b48bc931..9188d81c 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -121,7 +121,7 @@ Motor_t motors[] = { .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { .encoder_timer = &htim3, - .use_index = true, + .use_index = false, .index_found = false, .calibrated = false, .idx_search_speed = 10.0f, // [rad/s electrical] @@ -222,7 +222,7 @@ Motor_t motors[] = { .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { .encoder_timer = &htim4, - .use_index = true, + .use_index = false, .index_found = false, .calibrated = false, .idx_search_speed = 10.0f, // [rad/s electrical] From 03d90035e39c81fef24233342b76e81f1364a2b9 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 11 Feb 2018 13:34:21 -0800 Subject: [PATCH 155/155] Update protocol.py --- tools/odrive/protocol.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index a8586667..9d69aa08 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -11,7 +11,7 @@ if sys.version_info >= (3, 4): else: ABC = abc.ABCMeta('ABC', (), {}) -if sys.version_info <= (3,4): +if sys.version_info <= (3, 3): from monotonic import monotonic time.monotonic = monotonic