Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-25 11:06:48 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #56 from madcowswe/fw-v0.2-rc

Browse Source 
Firmware version 0.2 release.
This commit is contained in:
Oskar Weigl
2017-11-12 15:22:34 -08:00
committed by GitHub
parent 32b95d1de9 60f0873bc3
commit b8a94fbef3
49 changed files with 4964 additions and 1266 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/.vscode/c_cpp_properties.json
Vendored
+47 -1
View File
@@ -15,7 +15,53 @@
"${workspaceRoot}/Drivers/CMSIS/Device/ST/STM32F4xx/Include",
"${workspaceRoot}/Drivers/CMSIS/Include",
"${workspaceRoot}/Inc",
"${workspaceRoot}/MotorControl"
"${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": ""
}
},
{
"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",
Firmware/CHANGELOG.md
+7 -1
View File
@@ -1,5 +1,11 @@
## [0.1] - UNRELEASED
## [0.2.0] - 2017-11-12
### Added
* UART communication
* Setting to select UART or Step/dir on GIPIO 1,2
* Basic Anti-cogging
## [0.1.0] - 2017-08-26
### Added
* Step/Dir interface
* this Changelog
Firmware/Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c
-763
View File
File diff suppressed because it is too large Load Diff
Firmware/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h
+794
View File
File diff suppressed because it is too large Load Diff
Firmware/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c
+2539
View File
File diff suppressed because it is too large Load Diff
Firmware/Inc/dma.h
+88
View File
@@ -0,0 +1,88 @@
/**
******************************************************************************
* File Name : dma.h
* Description : This file contains all the function prototypes for
* the dma.c file
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2017 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __dma_H
#define __dma_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* DMA memory to memory transfer handles -------------------------------------*/
extern void _Error_Handler(char*, int);
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_DMA_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /* __dma_H */
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Firmware/Inc/freertos_vars.h
+1 -1
View File
@@ -8,6 +8,6 @@ osSemaphoreId sem_usb_irq;
// List of threads
osThreadId thread_motor_0;
osThreadId thread_motor_1;
osThreadId thread_usb_cmd;
osThreadId thread_cmd_parse;
#endif /* __FREERTOS_H */
Firmware/Inc/gpio.h
+3
View File
@@ -70,6 +70,9 @@ void MX_GPIO_Init(void);
/* USER CODE BEGIN Prototypes */
void SetGPIO12toUART();
void SetGPIO12toStepDir();
/* USER CODE END Prototypes */
#ifdef __cplusplus
Firmware/Inc/main.h
-1
View File
@@ -85,7 +85,6 @@
#define M1_IB_GPIO_Port GPIOC
#define GPIO_1_Pin GPIO_PIN_0
#define GPIO_1_GPIO_Port GPIOA
#define GPIO_1_EXTI_IRQn EXTI0_IRQn
#define GPIO_2_Pin GPIO_PIN_1
#define GPIO_2_GPIO_Port GPIOA
#define GPIO_3_Pin GPIO_PIN_2
Firmware/Inc/stm32f4xx_hal_conf.h
+2 -2
View File
@@ -76,7 +76,7 @@
/* #define HAL_MMC_MODULE_ENABLED */
#define HAL_SPI_MODULE_ENABLED
#define HAL_TIM_MODULE_ENABLED
/* #define HAL_UART_MODULE_ENABLED */
#define HAL_UART_MODULE_ENABLED
/* #define HAL_USART_MODULE_ENABLED */
/* #define HAL_IRDA_MODULE_ENABLED */
/* #define HAL_SMARTCARD_MODULE_ENABLED */
@@ -157,7 +157,7 @@
* @brief This is the HAL system configuration section
*/
#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY ((uint32_t)15U) /*!< tick interrupt priority */
#define TICK_INT_PRIORITY ((uint32_t)0U) /*!< tick interrupt priority */
#define USE_RTOS 0U
#define PREFETCH_ENABLE 1U
#define INSTRUCTION_CACHE_ENABLE 1U
Firmware/Inc/stm32f4xx_it.h
+6 -2
View File
@@ -40,6 +40,8 @@
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
@@ -52,9 +54,11 @@ void BusFault_Handler(void);
void UsageFault_Handler(void);
void DebugMon_Handler(void);
void SysTick_Handler(void);
void EXTI0_IRQHandler(void);
void EXTI2_IRQHandler(void);
void DMA1_Stream2_IRQHandler(void);
void DMA1_Stream4_IRQHandler(void);
void ADC_IRQHandler(void);
void TIM8_TRG_COM_TIM14_IRQHandler(void);
void UART4_IRQHandler(void);
void OTG_FS_IRQHandler(void);
#ifdef __cplusplus
Firmware/Inc/usart.h
+91
View File
@@ -0,0 +1,91 @@
/**
******************************************************************************
* File Name : USART.h
* Description : This file provides code for the configuration
* of the USART instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2017 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __usart_H
#define __usart_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern UART_HandleTypeDef huart4;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
extern void _Error_Handler(char *, int);
void MX_UART4_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /*__ usart_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Firmware/Makefile
+10 -7
View File
@@ -13,9 +13,7 @@ TARGET = ODriveFirmware
# debug build?
DEBUG = 1
# optimization
# OPT = -O3 -ffast-math -flto
# OPT = -O3 -ffast-math
OPT = -O0 -ffast-math -u _printf_float -u _scanf_float
OPT = -Og -ffast-math
#######################################
# pathes
@@ -42,14 +40,17 @@ C_SOURCES = \
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c \
Src/system_stm32f4xx.c \
Src/stm32f4xx_it.c \
Src/stm32f4xx_hal_msp.c \
Src/stm32f4xx_hal_timebase_TIM.c \
Src/tim.c \
Src/gpio.c \
Src/main.c \
Src/adc.c \
Src/freertos.c \
Src/spi.c \
Src/stm32f4xx_hal_msp.c \
Src/can.c \
Src/usart.c \
Src/dma.c \
Src/usbd_conf.c \
Src/usbd_desc.c \
Src/usb_device.c \
@@ -74,11 +75,13 @@ C_SOURCES = \
Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c \
Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c \
Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c \
Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c \
Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c \
Src/usbd_cdc_if.c \
Src/syscalls.c \
MotorControl/utils.c \
MotorControl/low_level.c
MotorControl/commands.c \
MotorControl/low_level.c
ASM_SOURCES = \
startup/startup_stm32f405xx.s
@@ -134,7 +137,7 @@ LDSCRIPT = STM32F405RGTx_FLASH.ld
# libraries
LIBS = -lc -lm -lnosys -larm_cortexM4lf_math
LIBDIR = -LDrivers/CMSIS/Lib
LDFLAGS = -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs $(OPT) -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections
LDFLAGS = -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float $(OPT) -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections
# default action: build all
all: $(BUILD_DIR)/$(TARGET).elf $(BUILD_DIR)/$(TARGET).hex $(BUILD_DIR)/$(TARGET).bin
@@ -152,7 +155,7 @@ vpath %.cpp $(sort $(dir $(CPP_SOURCES)))
OBJECTS += $(addprefix $(BUILD_DIR)/,$(notdir $(ASM_SOURCES:.s=.o)))
vpath %.s $(sort $(dir $(ASM_SOURCES)))
$(BUILD_DIR)/%.o: %.c Makefile | $(BUILD_DIR)
$(BUILD_DIR)/%.o: %.c Makefile | $(BUILD_DIR)
@$(CC) -c $(CFLAGS) -Wa,-a,-ad,-alms=$(BUILD_DIR)/$(notdir $(<:.c=.lst)) $< -o $@
$(BUILD_DIR)/%.o: %.s Makefile | $(BUILD_DIR)
Firmware/MotorControl/commands.c
+355
View File
@@ -0,0 +1,355 @@
/* Includes ------------------------------------------------------------------*/
#include <cmsis_os.h>
#include <commands.h>
#include <usart.h>
#include <gpio.h>
#include <freertos_vars.h>
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
// This automatically updates to the interface that most
// recently recieved a command. In the future we may want to separate
// debug printf and the main serial comms.
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_STEP_DIR; //GPIO 1,2 is M0 Step,Dir
// variables exposed to usb/serial interface via set/get/monitor
// Note: this will be depricated soon
static float* const exposed_floats[] = {
&vbus_voltage, // ro
NULL, //&elec_rad_per_enc, // ro
&motors[0].pos_setpoint, // rw
&motors[0].pos_gain, // rw
&motors[0].vel_setpoint, // rw
&motors[0].vel_gain, // rw
&motors[0].vel_integrator_gain, // rw
&motors[0].vel_integrator_current, // rw
&motors[0].vel_limit, // rw
&motors[0].current_setpoint, // rw
&motors[0].calibration_current, // rw
&motors[0].phase_inductance, // ro
&motors[0].phase_resistance, // ro
&motors[0].current_meas.phB, // ro
&motors[0].current_meas.phC, // ro
&motors[0].DC_calib.phB, // rw
&motors[0].DC_calib.phC, // rw
&motors[0].shunt_conductance, // rw
&motors[0].phase_current_rev_gain, // rw
&motors[0].current_control.current_lim, // rw
&motors[0].current_control.p_gain, // rw
&motors[0].current_control.i_gain, // rw
&motors[0].current_control.v_current_control_integral_d, // rw
&motors[0].current_control.v_current_control_integral_q, // rw
&motors[0].current_control.Ibus, // ro
&motors[0].encoder.phase, // ro
&motors[0].encoder.pll_pos, // rw
&motors[0].encoder.pll_vel, // rw
&motors[0].encoder.pll_kp, // rw
&motors[0].encoder.pll_ki, // rw
&motors[1].pos_setpoint, // rw
&motors[1].pos_gain, // rw
&motors[1].vel_setpoint, // rw
&motors[1].vel_gain, // rw
&motors[1].vel_integrator_gain, // rw
&motors[1].vel_integrator_current, // rw
&motors[1].vel_limit, // rw
&motors[1].current_setpoint, // rw
&motors[1].calibration_current, // rw
&motors[1].phase_inductance, // ro
&motors[1].phase_resistance, // ro
&motors[1].current_meas.phB, // ro
&motors[1].current_meas.phC, // ro
&motors[1].DC_calib.phB, // rw
&motors[1].DC_calib.phC, // rw
&motors[1].shunt_conductance, // rw
&motors[1].phase_current_rev_gain, // rw
&motors[1].current_control.current_lim, // rw
&motors[1].current_control.p_gain, // rw
&motors[1].current_control.i_gain, // rw
&motors[1].current_control.v_current_control_integral_d, // rw
&motors[1].current_control.v_current_control_integral_q, // rw
&motors[1].current_control.Ibus, // ro
&motors[1].encoder.phase, // ro
&motors[1].encoder.pll_pos, // rw
&motors[1].encoder.pll_vel, // rw
&motors[1].encoder.pll_kp, // rw
&motors[1].encoder.pll_ki, // rw
};
static int* const 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[1].control_mode, // rw
&motors[1].encoder.encoder_offset, // rw
&motors[1].encoder.encoder_state, // ro
&motors[1].error, // rw
};
static bool* const exposed_bools[] = {
&motors[0].thread_ready, // ro
&motors[0].enable_control, // rw
&motors[0].do_calibration, // rw
&motors[0].calibration_ok, // ro
&motors[1].thread_ready, // ro
&motors[1].enable_control, // rw
&motors[1].do_calibration, // rw
&motors[1].calibration_ok, // ro
};
static uint16_t* const exposed_uint16[] = {
&motors[0].control_deadline, // rw
&motors[0].last_cpu_time, // ro
&motors[1].control_deadline, // rw
&motors[1].last_cpu_time, // ro
};
/* Private variables ---------------------------------------------------------*/
monitoring_slot monitoring_slots[20] = {0};
/* Private function prototypes -----------------------------------------------*/
static void print_monitoring(int limit);
/* Function implementations --------------------------------------------------*/
void init_communication() {
switch (gpio_mode) {
case GPIO_MODE_UART: {
SetGPIO12toUART();
} break;
case GPIO_MODE_STEP_DIR: {
SetGPIO12toStepDir();
}
}
}
void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface) {
// Set response interface
serial_printf_select = response_interface;
// TODO very hacky way of terminating sscanf at end of buffer:
// We should do some proper struct packing instead of using sscanf altogether
buffer[len-1] = 0;
// check incoming packet type
if (buffer[0] == 'p') {
// position control
unsigned motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
if (numscan == 4 && motor_number < num_motors) {
set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'v') {
// velocity control
unsigned motor_number;
float vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
if (numscan == 3 && motor_number < num_motors) {
set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'c') {
// current control
unsigned motor_number;
float current_feed_forward;
int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, &current_feed_forward);
if (numscan == 2 && motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
} else if (buffer[0] == 'g') { // GET
// g <0:float,1:int,2:bool,3:uint16> index
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index);
if (numscan == 2) {
switch(type){
case 0: {
printf("%f\n",*exposed_floats[index]);
break;
};
case 1: {
printf("%d\n",*exposed_ints[index]);
break;
};
case 2: {
printf("%d\n",*exposed_bools[index]);
break;
};
case 3: {
printf("%hu\n",*exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 'h'){ // HALT
for(int i = 0; i < num_motors; i++){
set_vel_setpoint(&motors[i], 0.0f, 0.0f);
}
} else if (buffer[0] == 's') { // SET
// s <0:float,1:int,2:bool,3:uint16> index value
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index);
if (numscan == 2) {
switch(type) {
case 0: {
sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]);
break;
};
case 1: {
sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]);
break;
};
case 2: {
int btmp = 0;
sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp);
*exposed_bools[index] = btmp ? true : false;
break;
};
case 3: {
sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 'm') { // Setup Monitor
// m <0:float,1:int,2:bool,3:uint16> index monitoring_slot
int type = 0;
int index = 0;
int slot = 0;
int numscan = sscanf((const char*)buffer, "m %u %u %u", &type, &index, &slot);
if (numscan == 3) {
monitoring_slots[slot].type = type;
monitoring_slots[slot].index = index;
}
} else if (buffer[0] == 'o') { // Output Monitor
int limit = 0;
int numscan = sscanf((const char*)buffer, "o %u", &limit);
if (numscan == 1) {
print_monitoring(limit);
}
} else if (buffer[0] == 't') { // Run Anti-Cogging Calibration
for (int i = 0; i < num_motors; i++) {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (motors[i].anticogging.cogging_map != NULL && motors[i].error == ERROR_NO_ERROR) {
motors[i].anticogging.calib_anticogging = true;
}
}
}
}
static void print_monitoring(int limit) {
for (int i=0;i<limit;i++) {
switch (monitoring_slots[i].type) {
case 0:
printf("%f\t",*exposed_floats[monitoring_slots[i].index]);
break;
case 1:
printf("%d\t",*exposed_ints[monitoring_slots[i].index]);
break;
case 2:
printf("%d\t",*exposed_bools[monitoring_slots[i].index]);
break;
case 3:
printf("%hu\t",*exposed_uint16[monitoring_slots[i].index]);
break;
default:
i=100;
}
}
printf("\n");
}
// Thread to handle deffered processing of USB interrupt, and
// read commands out of the UART DMA circular buffer
void cmd_parse_thread(void const * argument) {
//DMA open loop continous circular buffer
//1ms delay periodic, chase DMA ptr around, on new data:
// Check for start char
// copy into parse-buffer
// check for end-char
// checksum, etc.
#define UART_RX_BUFFER_SIZE 64
static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE];
static uint8_t parse_buffer[UART_RX_BUFFER_SIZE];
// DMA is set up to recieve in a circular buffer forever.
// We dont use interrupts to fetch the data, instead we periodically read
// 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;
// Re-run state-machine forever
for (;;) {
//Inialize recieve state machine
bool reset_read_state = false;
bool read_active = false;
uint32_t parse_buffer_idx = 0;
//Run state machine until reset
do {
// 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));
break; //reset state machine
}
// 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;
// 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: exchange end char with terminating null
parse_buffer[parse_buffer_idx-1] = '\0';
motor_parse_cmd(parse_buffer, parse_buffer_idx, SERIAL_PRINTF_IS_UART);
// Reset receieve state machine
reset_read_state = true;
break;
} else if (parse_buffer_idx == UART_RX_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.
reset_read_state = true;
break;
}
}
}
// 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);
}
} while (!reset_read_state);
}
// If we get here, then this task is done
vTaskDelete(osThreadGetId());
}
Firmware/MotorControl/commands.h
+30
View File
@@ -0,0 +1,30 @@
#ifndef COMMANDS_H
#define COMMANDS_H
/* Includes ------------------------------------------------------------------*/
#include <low_level.h>
/* Exported types ------------------------------------------------------------*/
typedef enum {
GPIO_MODE_UART,
GPIO_MODE_STEP_DIR,
} GpioMode_t;
typedef enum {
SERIAL_PRINTF_IS_NONE,
SERIAL_PRINTF_IS_USB,
SERIAL_PRINTF_IS_UART,
} SerialPrintf_t;
/* Exported constants --------------------------------------------------------*/
/* Exported variables --------------------------------------------------------*/
extern SerialPrintf_t serial_printf_select;
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void init_communication();
void cmd_parse_thread(void const * argument);
void motor_parse_cmd(uint8_t* buffer, int len, SerialPrintf_t response_interface);
#endif /* COMMANDS_H */
Firmware/MotorControl/low_level.c
+102 -252
View File
@@ -2,12 +2,14 @@
// Because of broken cmsis_os.h, we need to include arm_math first,
// otherwise chip specific defines are ommited
#include <stm32f405xx.h>
#include <stm32f4xx_hal.h> // Sets up the correct chip specifc defines required by arm_math
#define ARM_MATH_CM4
#include <arm_math.h>
#include <low_level.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <cmsis_os.h>
@@ -93,6 +95,7 @@ Motor_t motors[] = {
.Ibus = 0.0f,
.final_v_alpha = 0.0f,
.final_v_beta = 0.0f,
.Iq = 0.0f,
},
// .rotor_mode = ROTOR_MODE_SENSORLESS,
// .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS,
@@ -125,6 +128,14 @@ Motor_t motors[] = {
},
.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
@@ -176,6 +187,7 @@ Motor_t motors[] = {
.Ibus = 0.0f,
.final_v_alpha = 0.0f,
.final_v_beta = 0.0f,
.Iq = 0.0f,
},
.rotor_mode = ROTOR_MODE_ENCODER,
.encoder = {
@@ -205,7 +217,15 @@ Motor_t motors[] = {
.spin_up_target_vel = 400.0f, // [rad/s]
},
.timing_log_index = 0,
.timing_log = {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 int num_motors = sizeof(motors)/sizeof(motors[0]);
@@ -219,107 +239,9 @@ static const int current_meas_hz = CURRENT_MEAS_HZ;
/* Private variables ---------------------------------------------------------*/
static float brake_resistance = 0.47f; // [ohm]
/* Monitoring */
monitoring_slot monitoring_slots[20] = {0};
/* variables exposed to usb interface via set/get/monitor
* If you change something here, don't forget to regenerate the python interface with generate_api.py
* ro/rw : read only/read write -> ro prevents the code generator from generating setter
* */
float* exposed_floats[] = {
&vbus_voltage, // ro
&elec_rad_per_enc, // ro
&motors[0].pos_setpoint, // rw
&motors[0].pos_gain, // rw
&motors[0].vel_setpoint, // rw
&motors[0].vel_gain, // rw
&motors[0].vel_integrator_gain, // rw
&motors[0].vel_integrator_current, // rw
&motors[0].vel_limit, // rw
&motors[0].current_setpoint, // rw
&motors[0].calibration_current, // rw
&motors[0].phase_inductance, // ro
&motors[0].phase_resistance, // ro
&motors[0].current_meas.phB, // ro
&motors[0].current_meas.phC, // ro
&motors[0].DC_calib.phB, // rw
&motors[0].DC_calib.phC, // rw
&motors[0].shunt_conductance, // rw
&motors[0].phase_current_rev_gain, // rw
&motors[0].current_control.current_lim, // rw
&motors[0].current_control.p_gain, // rw
&motors[0].current_control.i_gain, // rw
&motors[0].current_control.v_current_control_integral_d, // rw
&motors[0].current_control.v_current_control_integral_q, // rw
&motors[0].current_control.Ibus, // ro
&motors[0].encoder.phase, // ro
&motors[0].encoder.pll_pos, // rw
&motors[0].encoder.pll_vel, // rw
&motors[0].encoder.pll_kp, // rw
&motors[0].encoder.pll_ki, // rw
&motors[1].pos_setpoint, // rw
&motors[1].pos_gain, // rw
&motors[1].vel_setpoint, // rw
&motors[1].vel_gain, // rw
&motors[1].vel_integrator_gain, // rw
&motors[1].vel_integrator_current, // rw
&motors[1].vel_limit, // rw
&motors[1].current_setpoint, // rw
&motors[1].calibration_current, // rw
&motors[1].phase_inductance, // ro
&motors[1].phase_resistance, // ro
&motors[1].current_meas.phB, // ro
&motors[1].current_meas.phC, // ro
&motors[1].DC_calib.phB, // rw
&motors[1].DC_calib.phC, // rw
&motors[1].shunt_conductance, // rw
&motors[1].phase_current_rev_gain, // rw
&motors[1].current_control.current_lim, // rw
&motors[1].current_control.p_gain, // rw
&motors[1].current_control.i_gain, // rw
&motors[1].current_control.v_current_control_integral_d, // rw
&motors[1].current_control.v_current_control_integral_q, // rw
&motors[1].current_control.Ibus, // ro
&motors[1].encoder.phase, // ro
&motors[1].encoder.pll_pos, // rw
&motors[1].encoder.pll_vel, // rw
&motors[1].encoder.pll_kp, // rw
&motors[1].encoder.pll_ki, // rw
};
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[1].control_mode, // rw
&motors[1].encoder.encoder_offset, // rw
&motors[1].encoder.encoder_state, // ro
&motors[1].error, // rw
};
bool* exposed_bools[] = {
&motors[0].thread_ready, // ro
&motors[0].enable_control, // rw
&motors[0].do_calibration, // rw
&motors[0].calibration_ok, // ro
&motors[1].thread_ready, // ro
&motors[1].enable_control, // rw
&motors[1].do_calibration, // rw
&motors[1].calibration_ok, // ro
};
uint16_t* exposed_uint16[] = {
&motors[0].control_deadline, // rw
&motors[0].last_cpu_time, // ro
&motors[1].control_deadline, // rw
&motors[1].last_cpu_time, // ro
};
/* Private function prototypes -----------------------------------------------*/
// Command Handling
static void print_monitoring(int limit);
// Utility
static uint16_t check_timing(Motor_t* motor);
static void global_fault(int error);
@@ -359,27 +281,7 @@ static void control_motor_loop(Motor_t* motor);
// TODO move to different file
//--------------------------------
static void print_monitoring(int limit) {
for (int i=0;i<limit;i++) {
switch (monitoring_slots[i].type) {
case 0:
printf("%f\t",*exposed_floats[monitoring_slots[i].index]);
break;
case 1:
printf("%d\t",*exposed_ints[monitoring_slots[i].index]);
break;
case 2:
printf("%d\t",*exposed_bools[monitoring_slots[i].index]);
break;
case 3:
printf("%hu\t",*exposed_uint16[monitoring_slots[i].index]);
break;
default:
i=100;
}
}
printf("\n");
}
void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
motor->pos_setpoint = pos_setpoint;
@@ -408,109 +310,6 @@ void set_current_setpoint(Motor_t* motor, float current_setpoint) {
#endif
}
void motor_parse_cmd(uint8_t* buffer, int len) {
// TODO very hacky way of terminating sscanf at end of buffer:
// We should do some proper struct packing instead of using sscanf altogether
buffer[len] = 0;
// check incoming packet type
if (buffer[0] == 'p') {
// position control
unsigned motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
if (numscan == 4 && motor_number < num_motors) {
set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'v') {
// velocity control
unsigned motor_number;
float vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
if (numscan == 3 && motor_number < num_motors) {
set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'c') {
// current control
unsigned motor_number;
float current_feed_forward;
int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, &current_feed_forward);
if (numscan == 2 && motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
} else if (buffer[0] == 'g') { // GET
// g <0:float,1:int,2:bool,3:uint16> index
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index);
if (numscan == 2) {
switch(type){
case 0: {
printf("%f\n",*exposed_floats[index]);
break;
};
case 1: {
printf("%d\n",*exposed_ints[index]);
break;
};
case 2: {
printf("%d\n",*exposed_bools[index]);
break;
};
case 3: {
printf("%hu\n",*exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 's') { // SET
// s <0:float,1:int,2:bool,3:uint16> index value
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index);
if (numscan == 2) {
switch(type) {
case 0: {
sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]);
break;
};
case 1: {
sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]);
break;
};
case 2: {
int btmp = 0;
sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp);
*exposed_bools[index] = btmp ? true : false;
break;
};
case 3: {
sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 'm') { // Setup Monitor
// m <0:float,1:int,2:bool,3:uint16> index monitoring_slot
int type = 0;
int index = 0;
int slot = 0;
int numscan = sscanf((const char*)buffer, "m %u %u %u", &type, &index, &slot);
if (numscan == 3) {
monitoring_slots[slot].type = type;
monitoring_slots[slot].index = index;
}
} else if (buffer[0] == 'o') { // Output Monitor
int limit = 0;
int numscan = sscanf((const char*)buffer, "o %u", &limit);
if (numscan == 1) {
print_monitoring(limit);
}
}
}
//--------------------------------
// Utility
//--------------------------------
@@ -1038,16 +837,44 @@ static bool motor_calibration(Motor_t* motor){
// sensorless pll same as encoder (for now)
motor->sensorless.pll_kp = motor->encoder.pll_kp;
motor->sensorless.pll_ki = motor->encoder.pll_ki;
motor->calibration_ok = true;
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))
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) {
@@ -1067,6 +894,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) {
for (;;) {
osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever);
@@ -1212,36 +1040,39 @@ static void update_rotor(Motor_t* motor) {
}
}
static 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;
}
static bool using_sensorless(Motor_t* motor) {
if (motor->rotor_mode == ROTOR_MODE_SENSORLESS)
return true;
else
return false;
}
static float get_rotor_phase(Motor_t* motor) {
switch (motor->rotor_mode) {
case ROTOR_MODE_ENCODER:
case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS:
return motor->encoder.phase;
break;
case ROTOR_MODE_SENSORLESS:
return motor->sensorless.phase;
break;
default:
//TODO error handling
return 0.0f;
break;
}
if (using_encoder(motor))
return motor->encoder.phase;
else if (using_sensorless(motor))
return motor->sensorless.phase;
else
//TODO error handling
return 0.0f;
}
static float get_pll_vel(Motor_t* motor) {
switch (motor->rotor_mode) {
case ROTOR_MODE_ENCODER:
case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS:
return motor->encoder.pll_vel;
break;
case ROTOR_MODE_SENSORLESS:
return motor->sensorless.pll_vel;
break;
default:
//TODO error handling
return 0.0f;
break;
}
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;
}
static bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) {
@@ -1418,6 +1249,7 @@ static void control_motor_loop(Motor_t* motor) {
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
@@ -1438,6 +1270,14 @@ static void control_motor_loop(Motor_t* motor) {
// 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;
@@ -1477,6 +1317,7 @@ 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
@@ -1494,6 +1335,15 @@ static void control_motor_loop(Motor_t* motor) {
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[i] = 0.0f;
}
}
motor->motor_thread = osThreadGetId();
motor->thread_ready = true;
@@ -1504,7 +1354,7 @@ void motor_thread(void const * argument) {
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor->motor_timer);// disables pwm outputs
motor->do_calibration = false;
}
if (motor->calibration_ok && motor->enable_control) {
motor->enable_step_dir = true;
__HAL_TIM_MOE_ENABLE(motor->motor_timer);
Firmware/MotorControl/low_level.h
+15 -6
View File
@@ -14,6 +14,15 @@ typedef enum {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
} Motor_thread_signals_t;
typedef struct {
int index;
float *cogging_map;
bool use_anticogging;
bool calib_anticogging;
float calib_pos_threshold;
float calib_vel_threshold;
} Anticogging_t;
typedef enum {
ERROR_NO_ERROR,
ERROR_PHASE_RESISTANCE_TIMING,
@@ -61,6 +70,7 @@ typedef struct {
// Voltage applied at end of cycle:
float final_v_alpha; // [V]
float final_v_beta; // [V]
float Iq;
} Current_control_t;
typedef enum {
@@ -135,6 +145,7 @@ typedef struct {
Sensorless_t sensorless;
int timing_log_index;
uint16_t timing_log[TIMING_LOG_SIZE];
Anticogging_t anticogging;
} Motor_t;
typedef struct{
@@ -143,11 +154,10 @@ typedef struct{
} monitoring_slot;
/* Exported constants --------------------------------------------------------*/
extern const int num_motors;
/* Exported variables --------------------------------------------------------*/
extern float vbus_voltage;
extern Motor_t motors[];
extern const int num_motors;
/* Exported variables --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
@@ -162,10 +172,9 @@ 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);
bool anti_cogging_calibration(Motor_t* motor);
//@TODO move motor thread to high level file
void motor_thread(void const * argument);
//@TODO move cmd parsing to high level file
void motor_parse_cmd(uint8_t* buffer, int len);
#endif //__LOW_LEVEL_H
Firmware/MotorControl/utils.c
+6
View File
@@ -161,3 +161,9 @@ float fast_atan2(float y, float x) {
return r;
}
// Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
int mod(int dividend, int divisor){
int r = dividend % divisor;
return (r < 0) ? (r + divisor) : r;
}
Firmware/MotorControl/utils.h
+1
View File
@@ -18,5 +18,6 @@ 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
Firmware/Odrive.ioc
+80 -40
View File
@@ -80,6 +80,29 @@ CAN1.BS2=CAN_BS2_1TQ
CAN1.CalculateTimeBit=1142
CAN1.CalculateTimeQuantum=380.95238095238096
CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,BS1,BS2
Dma.Request0=UART4_RX
Dma.Request1=UART4_TX
Dma.RequestsNb=2
Dma.UART4_RX.0.Direction=DMA_PERIPH_TO_MEMORY
Dma.UART4_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.UART4_RX.0.Instance=DMA1_Stream2
Dma.UART4_RX.0.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.UART4_RX.0.MemInc=DMA_MINC_ENABLE
Dma.UART4_RX.0.Mode=DMA_CIRCULAR
Dma.UART4_RX.0.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.UART4_RX.0.PeriphInc=DMA_PINC_DISABLE
Dma.UART4_RX.0.Priority=DMA_PRIORITY_LOW
Dma.UART4_RX.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
Dma.UART4_TX.1.Direction=DMA_MEMORY_TO_PERIPH
Dma.UART4_TX.1.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.UART4_TX.1.Instance=DMA1_Stream4
Dma.UART4_TX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.UART4_TX.1.MemInc=DMA_MINC_ENABLE
Dma.UART4_TX.1.Mode=DMA_NORMAL
Dma.UART4_TX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.UART4_TX.1.PeriphInc=DMA_PINC_DISABLE
Dma.UART4_TX.1.Priority=DMA_PRIORITY_LOW
Dma.UART4_TX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
FREERTOS.INCLUDE_vTaskDelayUntil=1
FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelayUntil
FREERTOS.Tasks01=defaultTask,-3,256,StartDefaultTask,Default
@@ -88,21 +111,23 @@ KeepUserPlacement=true
Mcu.Family=STM32F4
Mcu.IP0=ADC1
Mcu.IP1=ADC2
Mcu.IP10=TIM2
Mcu.IP11=TIM3
Mcu.IP12=TIM4
Mcu.IP13=TIM8
Mcu.IP14=USB_DEVICE
Mcu.IP15=USB_OTG_FS
Mcu.IP10=TIM1
Mcu.IP11=TIM2
Mcu.IP12=TIM3
Mcu.IP13=TIM4
Mcu.IP14=TIM8
Mcu.IP15=UART4
Mcu.IP16=USB_DEVICE
Mcu.IP17=USB_OTG_FS
Mcu.IP2=ADC3
Mcu.IP3=CAN1
Mcu.IP4=FREERTOS
Mcu.IP5=NVIC
Mcu.IP6=RCC
Mcu.IP7=SPI3
Mcu.IP8=SYS
Mcu.IP9=TIM1
Mcu.IPNb=16
Mcu.IP4=DMA
Mcu.IP5=FREERTOS
Mcu.IP6=NVIC
Mcu.IP7=RCC
Mcu.IP8=SPI3
Mcu.IP9=SYS
Mcu.IPNb=18
Mcu.Name=STM32F405RGTx
Mcu.Package=LQFP64
Mcu.Pin0=PC13-ANTI_TAMP
@@ -153,7 +178,7 @@ Mcu.Pin49=PB8
Mcu.Pin5=PC0
Mcu.Pin50=PB9
Mcu.Pin51=VP_FREERTOS_VS_ENABLE
Mcu.Pin52=VP_SYS_VS_Systick
Mcu.Pin52=VP_SYS_VS_tim14
Mcu.Pin53=VP_TIM1_VS_ClockSourceINT
Mcu.Pin54=VP_TIM1_VS_no_output4
Mcu.Pin55=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
@@ -164,32 +189,39 @@ Mcu.Pin9=PA0-WKUP
Mcu.PinsNb=56
Mcu.UserConstants=TIM_1_8_CLOCK_HZ,168000000;TIM_1_8_PERIOD_CLOCKS,10192;TIM_1_8_DEADTIME_CLOCKS,20;TIM_APB1_CLOCK_HZ,84000000;TIM_APB1_PERIOD_CLOCKS,4096;TIM_APB1_DEADTIME_CLOCKS,40
Mcu.UserName=STM32F405RGTx
MxCube.Version=4.22.0
MxDb.Version=DB.4.0.220
NVIC.ADC_IRQn=true\:5\:0\:false\:false\:true\:true
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false
NVIC.EXTI0_IRQn=true\:0\:0\:false\:false\:true\:false
NVIC.EXTI2_IRQn=true\:0\:0\:false\:false\:true\:false
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false
NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false
NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false
NVIC.OTG_FS_IRQn=true\:5\:0\:false\:false\:true\:true
NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true
MxCube.Version=4.22.1
MxDb.Version=DB.4.0.221
NVIC.ADC_IRQn=true\:5\:0\:false\:false\:true\:true\:true
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true
NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:true
NVIC.DMA1_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:true
NVIC.EXTI2_IRQn=true\:0\:0\:false\:false\:false\:false\:true
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true
NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:true
NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:true
NVIC.OTG_FS_IRQn=true\:5\:0\:false\:false\:true\:true\:true
NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:true
NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false
NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:true
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:true
NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:true\:true
NVIC.TIM8_TRG_COM_TIM14_IRQn=true\:0\:0\:false\:false\:true\:false\:false
NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn
NVIC.TimeBaseIP=TIM14
NVIC.UART4_IRQn=true\:5\:0\:false\:false\:true\:true\:true
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true
PA0-WKUP.GPIOParameters=GPIO_PuPd,GPIO_Label
PA0-WKUP.GPIO_Label=GPIO_1
PA0-WKUP.GPIO_PuPd=GPIO_PULLDOWN
PA0-WKUP.Locked=true
PA0-WKUP.Signal=GPXTI0
PA0-WKUP.Signal=SharedStack_PA0
PA0-WKUP.Stacked=true
PA1.GPIOParameters=GPIO_PuPd,GPIO_Label
PA1.GPIO_Label=GPIO_2
PA1.GPIO_PuPd=GPIO_NOPULL
PA1.Locked=true
PA1.Signal=GPIO_Input
PA1.Signal=SharedStack_PA1
PA1.Stacked=true
PA10.GPIOParameters=GPIO_Label
PA10.GPIO_Label=M0_CH
PA10.Locked=true
@@ -398,7 +430,7 @@ ProjectManager.StackSize=0x800
ProjectManager.TargetToolchain=SW4STM32
ProjectManager.ToolChainLocation=
ProjectManager.UnderRoot=true
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL,2-MX_ADC1_Init-ADC1-false-HAL,3-MX_ADC2_Init-ADC2-false-HAL,4-MX_CAN1_Init-CAN1-false-HAL,5-MX_TIM1_Init-TIM1-false-HAL,6-MX_TIM8_Init-TIM8-false-HAL,7-MX_TIM3_Init-TIM3-false-HAL,8-MX_TIM4_Init-TIM4-false-HAL,9-MX_SPI3_Init-SPI3-false-HAL,10-MX_ADC3_Init-ADC3-false-HAL,11-SystemClock_Config-RCC-false-HAL,12-MX_TIM2_Init-TIM2-false-HAL,13-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL,2-MX_DMA_Init-DMA-false-HAL,3-MX_ADC1_Init-ADC1-false-HAL,4-MX_ADC2_Init-ADC2-false-HAL,5-MX_CAN1_Init-CAN1-false-HAL,6-MX_TIM1_Init-TIM1-false-HAL,7-MX_TIM8_Init-TIM8-false-HAL,8-MX_TIM3_Init-TIM3-false-HAL,9-MX_TIM4_Init-TIM4-false-HAL,10-MX_SPI3_Init-SPI3-false-HAL,11-MX_ADC3_Init-ADC3-false-HAL,12-SystemClock_Config-RCC-false-HAL,13-MX_TIM2_Init-TIM2-false-HAL,14-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL,15-MX_UART4_Init-UART4-false-HAL
RCC.48MHZClocksFreq_Value=48000000
RCC.AHBFreq_Value=168000000
RCC.APB1CLKDivider=RCC_HCLK_DIV4
@@ -464,8 +496,6 @@ SH.ADCx_IN5.ConfNb=2
SH.ADCx_IN6.0=ADC1_IN6,IN6
SH.ADCx_IN6.1=ADC2_IN6,IN6
SH.ADCx_IN6.ConfNb=2
SH.GPXTI0.0=GPIO_EXTI0
SH.GPXTI0.ConfNb=1
SH.GPXTI2.0=GPIO_EXTI2
SH.GPXTI2.ConfNb=1
SH.S_TIM1_CH1.0=TIM1_CH1,PWM Generation1 CH1 CH1N
@@ -492,6 +522,12 @@ SH.S_TIM8_CH2.0=TIM8_CH2,PWM Generation2 CH2 CH2N
SH.S_TIM8_CH2.ConfNb=1
SH.S_TIM8_CH3.0=TIM8_CH3,PWM Generation3 CH3 CH3N
SH.S_TIM8_CH3.ConfNb=1
SH.SharedStack_PA0.0=GPIO_EXTI0+0
SH.SharedStack_PA0.1=UART4_TX,Asynchronous
SH.SharedStack_PA0.ConfNb=2
SH.SharedStack_PA1.0=GPIO_Input+0
SH.SharedStack_PA1.1=UART4_RX,Asynchronous
SH.SharedStack_PA1.ConfNb=2
SPI3.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_16
SPI3.CLKPhase=SPI_PHASE_2EDGE
SPI3.CalculateBaudRate=2.625 MBits/s
@@ -555,11 +591,15 @@ TIM8.OffStateIDLEMode=TIM_OSSI_ENABLE
TIM8.OffStateRunMode=TIM_OSSR_ENABLE
TIM8.Period=TIM_1_8_PERIOD_CLOCKS
TIM8.TIM_MasterOutputTrigger=TIM_TRGO_UPDATE
UART4.IPParameters=VirtualMode
UART4.VirtualMode=Asynchronous
USB_DEVICE.APP_RX_DATA_SIZE-CDC_FS=64
USB_DEVICE.APP_TX_DATA_SIZE-CDC_FS=64
USB_DEVICE.CLASS_NAME_FS=CDC
USB_DEVICE.IPParameters=VirtualMode-CDC_FS,VirtualModeFS,CLASS_NAME_FS,MANUFACTURER_STRING-CDC_FS,PRODUCT_STRING_CDC_FS,VID-CDC_FS,PID_CDC_FS,SERIALNUMBER_STRING_CDC_FS
USB_DEVICE.MANUFACTURER_STRING-CDC_FS=ODrive
USB_DEVICE.PID_CDC_FS=0x0D31
USB_DEVICE.PRODUCT_STRING_CDC_FS=ODrive rev 3.1
USB_DEVICE.IPParameters=VirtualMode-CDC_FS,VirtualModeFS,CLASS_NAME_FS,MANUFACTURER_STRING-CDC_FS,PRODUCT_STRING_CDC_FS,VID-CDC_FS,PID_CDC_FS,SERIALNUMBER_STRING_CDC_FS,APP_RX_DATA_SIZE-CDC_FS,APP_TX_DATA_SIZE-CDC_FS
USB_DEVICE.MANUFACTURER_STRING-CDC_FS=ODrive Robotics
USB_DEVICE.PID_CDC_FS=0x0D33
USB_DEVICE.PRODUCT_STRING_CDC_FS=ODrive v3.3
USB_DEVICE.SERIALNUMBER_STRING_CDC_FS=000000000001
USB_DEVICE.VID-CDC_FS=0x1209
USB_DEVICE.VirtualMode-CDC_FS=Cdc
@@ -569,8 +609,8 @@ USB_OTG_FS.VirtualMode=Device_Only
USB_OTG_FS.vbus_sensing_enable=DISABLE
VP_FREERTOS_VS_ENABLE.Mode=Enabled
VP_FREERTOS_VS_ENABLE.Signal=FREERTOS_VS_ENABLE
VP_SYS_VS_Systick.Mode=SysTick
VP_SYS_VS_Systick.Signal=SYS_VS_Systick
VP_SYS_VS_tim14.Mode=TIM14
VP_SYS_VS_tim14.Signal=SYS_VS_tim14
VP_TIM1_VS_ClockSourceINT.Mode=Internal
VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT
VP_TIM1_VS_no_output4.Mode=Output Compare4 No Output
Firmware/README.md
+32 -20
View File
@@ -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 and UART](#communicating-over-usb-and-uart)
- [Generating startup code](#generating-startup-code)
- [Setting up Eclipse development environment](#setting-up-eclipse-development-environment)
- [Notes for Contributors](#notes-for-contributors)
@@ -20,26 +20,34 @@ If you are a developer, you are encouraged to use the `devel` branch, as it cont
## Configuring parameters
To correctly operate the ODrive, you need to supply some parameters. Some are mandatory, and if supplied incorrectly will cause the drive to malfunction. To get good performance you must also tune the drive.
### Hardware configuration
The first thing to set is your board hardware version, located at the top of [Inc/main.h](Inc/main.h). If, for example, you are using the hardware: ODrive v3.2, then you should set it like this:
```C
#define HW_VERSION_MAJOR 3
#define HW_VERSION_MINOR 2
```
### Communication configuration
If you are using USB only to communicate with the ODrive, you may 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`).
### 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.
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.
### Mandatory parameters
#### Mandatory parameters
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.
### Tuning parameters
#### 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 should increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains.
* 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 motion control gains are currently manually tuned:
@@ -56,7 +64,7 @@ An upcoming feature will enable automatic tuning. Until then, here is a rough tu
* Back down `pos_gain` until you do not have overshoot anymore.
* The integrator is not easily tuned, nor is it strictly required. Tune at your own discression.
### Optional parameters
#### Optional parameters
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.
@@ -77,18 +85,22 @@ To compile the program, you first need to install the prerequisite tools:
* Installing on Ubuntu: `sudo apt-get install openocd`
* No additional USB CDC driver should be required on Linux.
#### Mac:
* `brew cask install gcc-arm-embedded`: GCC toolchain+debugger
* `brew install openocd`: Programmer
#### Windows:
Install the following:
* [Git for windows](https://git-scm.com/download/win). This intalls the Git Bash, which is a unix style command line interface that we will be using.
* [GNU ARM Embedded Toolchain](https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads). The cross-compiler used to compile the code. Download and install the "Windows 32-bit" version. Make sure to tick the "add to path" option.
* [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`.
* [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).
### Building the firmware
* Make sure you have cloned the repository.
* Navigate your terminal (bash/cygwin) to the ODriveFirmware dir.
* Navigate your terminal (bash/cygwin) to the ODrive/Firmware dir.
* Run `make` in the root of this repository.
### Flashing the firmware
@@ -101,19 +113,25 @@ 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
## Communicating over USB and UART
There is currently a very primitive method to read/write configuration, commands and errors from the ODrive over the USB.
Please use the `ODriveFirmware/tools/test_bulk.py` python script for this. It is written for Python 3.
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 instlled first.
Setup instructions as follows:
* Install PyUSB (pip install --pre pysusb)
* Install PyUSB (pip install --pre pyusb). This can be done using the [gitbash terminal](https://git-for-windows.github.io/) for windows users.
* Plug in the STLink or another power source to power the ODrive board
* Plug in a separate USB cable into the microUSB connector on ODrive
* On Windows, use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb
* Run test_bulk.py
* On 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`
### Command set
The most accurate way to understand the commands is to read [the code](https://github.com/madcowswe/ODriveFirmware/blob/f19f1b78de4bd917284ff95bc61ca616ca9bacc4/MotorControl/low_level.c#L353) that parses the commands.
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
```
@@ -156,7 +174,7 @@ s type index value
** `0` is float
** `1` is int
** `2` is bool
* `index` is the index in the corresponding [exposed variable table](https://github.com/madcowswe/ODriveFirmware/blob/f19f1b78de4bd917284ff95bc61ca616ca9bacc4/MotorControl/low_level.c#L184-L265).
* `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
@@ -186,12 +204,6 @@ 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.
### Generate makefile
There is an excellent project called CubeMX2Makefile, originally from baoshi. This project is included as a submodule.
* Initialise and clone the submodules: `git submodule init; git submodule update`
* Generate makefile: `python2 CubeMX2Makefile/CubeMX2Makefile.py .`
## Setting up Eclipse development environment
### Install
Firmware/STM32F405RGTx_FLASH.ld
+7 -26
View File
@@ -18,32 +18,13 @@
** Distribution: The file is distributed as is, without any warranty
** of any kind.
**
*****************************************************************************
** @attention
**
** <h2><center>&copy; COPYRIGHT(c) 2014 Ac6</center></h2>
**
** Redistribution and use in source and binary forms, with or without modification,
** are permitted provided that the following conditions are met:
** 1. Redistributions of source code must retain the above copyright notice,
** this list of conditions and the following disclaimer.
** 2. Redistributions in binary form must reproduce the above copyright notice,
** this list of conditions and the following disclaimer in the documentation
** and/or other materials provided with the distribution.
** 3. Neither the name of Ac6 nor the names of its contributors
** may be used to endorse or promote products derived from this software
** without specific prior written permission.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
** (c)Copyright Ac6.
** You may use this file as-is or modify it according to the needs of your
** project. Distribution of this file (unmodified or modified) is not
** permitted. Ac6 permit registered System Workbench for MCU users the
** rights to distribute the assembled, compiled & linked contents of this
** file as part of an application binary file, provided that it is built
** using the System Workbench for MCU toolchain.
**
*****************************************************************************
*/
Firmware/Src/dma.c
+94
View File
@@ -0,0 +1,94 @@
/**
******************************************************************************
* File Name : dma.c
* Description : This file provides code for the configuration
* of all the requested memory to memory DMA transfers.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2017 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "dma.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/*----------------------------------------------------------------------------*/
/* Configure DMA */
/*----------------------------------------------------------------------------*/
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/**
* Enable DMA controller clock
*/
void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__HAL_RCC_DMA1_CLK_ENABLE();
/* DMA interrupt init */
/* DMA1_Stream2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn);
/* DMA1_Stream4_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn);
}
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Firmware/Src/freertos.c
+7 -23
View File
@@ -54,14 +54,14 @@
/* USER CODE BEGIN Includes */
#include "freertos_vars.h"
#include "low_level.h"
#include "version.h"
#include "commands.h"
/* USER CODE END Includes */
/* Variables -----------------------------------------------------------------*/
osThreadId defaultTaskHandle;
/* USER CODE BEGIN Variables */
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
/* USER CODE END Variables */
/* Function prototypes -------------------------------------------------------*/
@@ -71,7 +71,6 @@ extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/* USER CODE BEGIN FunctionPrototypes */
void usb_cmd_thread(void const * argument);
/* USER CODE END FunctionPrototypes */
@@ -121,6 +120,9 @@ void StartDefaultTask(void const * argument)
/* USER CODE BEGIN StartDefaultTask */
// Init communications
init_communication();
// Init motor control
init_motor_control();
@@ -131,8 +133,8 @@ void StartDefaultTask(void const * argument)
thread_motor_1 = osThreadCreate(osThread(task_motor_1), &motors[1]);
// Start USB command handling thread
osThreadDef(task_usb_cmd, usb_cmd_thread, osPriorityNormal, 0, 512);
thread_usb_cmd = osThreadCreate(osThread(task_usb_cmd), NULL);
osThreadDef(task_cmd_parse, cmd_parse_thread, osPriorityNormal, 0, 512);
thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL);
//If we get to here, then the default task is done.
vTaskDelete(defaultTaskHandle);
@@ -142,24 +144,6 @@ void StartDefaultTask(void const * argument)
/* USER CODE BEGIN Application */
// Thread to handle deffered processing of USB interrupt
void usb_cmd_thread(void const * argument) {
for (;;) {
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osSemaphoreWait(sem_usb_irq, osWaitForever);
// Irq processing loop
//while(HAL_NVIC_GetActive(OTG_FS_IRQn)) {
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
//}
// Let the irq (OTG_FS_IRQHandler) fire again.
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
}
// If we get here, then this task is done
vTaskDelete(osThreadGetId());
}
/* USER CODE END Application */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Firmware/Src/gpio.c
+60 -8
View File
@@ -71,6 +71,8 @@
* Output
* EVENT_OUT
* EXTI
PA0-WKUP ------> SharedStack_PA0
PA1 ------> SharedStack_PA1
*/
void MX_GPIO_Init(void)
{
@@ -100,14 +102,30 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pins : PAPin PAPin */
GPIO_InitStruct.Pin = GPIO_1_Pin|GPIO_3_Pin;
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GPIO_2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GPIO_3_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
HAL_GPIO_Init(GPIO_3_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pins : PAPin PAPin PAPin */
GPIO_InitStruct.Pin = GPIO_2_Pin|GPIO_4_Pin|M0_ENC_Z_Pin;
/*Configure GPIO pins : PAPin PAPin */
GPIO_InitStruct.Pin = GPIO_4_Pin|M0_ENC_Z_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
@@ -132,9 +150,6 @@ void MX_GPIO_Init(void)
HAL_GPIO_Init(nFAULT_GPIO_Port, &GPIO_InitStruct);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
HAL_NVIC_SetPriority(EXTI2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI2_IRQn);
@@ -143,6 +158,43 @@ void MX_GPIO_Init(void)
/* USER CODE BEGIN 2 */
#endif // End GPIO Include
void SetGPIO12toUART() {
GPIO_InitTypeDef GPIO_InitStruct;
HAL_NVIC_DisableIRQ(EXTI0_IRQn);
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
}
void SetGPIO12toStepDir() {
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
}
//Dispatch processing of external interrupts based on source
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
//Step signals for M0 and M1
Firmware/Src/main.c
+25
View File
@@ -51,8 +51,10 @@
#include "cmsis_os.h"
#include "adc.h"
#include "can.h"
#include "dma.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "usb_device.h"
#include "gpio.h"
@@ -105,6 +107,7 @@ int main(void)
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_ADC1_Init();
MX_ADC2_Init();
MX_CAN1_Init();
@@ -115,6 +118,7 @@ int main(void)
MX_SPI3_Init();
MX_ADC3_Init();
MX_TIM2_Init();
MX_UART4_Init();
/* USER CODE BEGIN 2 */
@@ -204,6 +208,27 @@ void SystemClock_Config(void)
/* USER CODE END 4 */
/**
* @brief Period elapsed callback in non blocking mode
* @note This function is called when TIM14 interrupt took place, inside
* HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
* a global variable "uwTick" used as application time base.
* @param htim : TIM handle
* @retval None
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
/* USER CODE BEGIN Callback 0 */
/* USER CODE END Callback 0 */
if (htim->Instance == TIM14) {
HAL_IncTick();
}
/* USER CODE BEGIN Callback 1 */
/* USER CODE END Callback 1 */
}
/**
* @brief This function is executed in case of error occurrence.
* @param None
Firmware/Src/stm32f4xx_hal_timebase_TIM.c
+158
View File
@@ -0,0 +1,158 @@
/**
******************************************************************************
* @file stm32f4xx_hal_timebase_TIM.c
* @brief HAL time base based on the hardware TIM.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2017 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "stm32f4xx_hal_tim.h"
/** @addtogroup STM32F7xx_HAL_Examples
* @{
*/
/** @addtogroup HAL_TimeBase
* @{
*/
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim14;
uint32_t uwIncrementState = 0;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/**
* @brief This function configures the TIM14 as a time base source.
* The time source is configured to have 1ms time base with a dedicated
* Tick interrupt priority.
* @note This function is called automatically at the beginning of program after
* reset by HAL_Init() or at any time when clock is configured, by HAL_RCC_ClockConfig().
* @param TickPriority: Tick interrupt priorty.
* @retval HAL status
*/
HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
{
RCC_ClkInitTypeDef clkconfig;
uint32_t uwTimclock = 0;
uint32_t uwPrescalerValue = 0;
uint32_t pFLatency;
/*Configure the TIM14 IRQ priority */
HAL_NVIC_SetPriority(TIM8_TRG_COM_TIM14_IRQn, TickPriority ,0);
/* Enable the TIM14 global Interrupt */
HAL_NVIC_EnableIRQ(TIM8_TRG_COM_TIM14_IRQn);
/* Enable TIM14 clock */
__HAL_RCC_TIM14_CLK_ENABLE();
/* Get clock configuration */
HAL_RCC_GetClockConfig(&clkconfig, &pFLatency);
/* Compute TIM14 clock */
uwTimclock = 2*HAL_RCC_GetPCLK1Freq();
/* Compute the prescaler value to have TIM14 counter clock equal to 1MHz */
uwPrescalerValue = (uint32_t) ((uwTimclock / 1000000) - 1);
/* Initialize TIM14 */
htim14.Instance = TIM14;
/* Initialize TIMx peripheral as follow:
+ Period = [(TIM14CLK/1000) - 1]. to have a (1/1000) s time base.
+ Prescaler = (uwTimclock/1000000 - 1) to have a 1MHz counter clock.
+ ClockDivision = 0
+ Counter direction = Up
*/
htim14.Init.Period = (1000000 / 1000) - 1;
htim14.Init.Prescaler = uwPrescalerValue;
htim14.Init.ClockDivision = 0;
htim14.Init.CounterMode = TIM_COUNTERMODE_UP;
if(HAL_TIM_Base_Init(&htim14) == HAL_OK)
{
/* Start the TIM time Base generation in interrupt mode */
return HAL_TIM_Base_Start_IT(&htim14);
}
/* Return function status */
return HAL_ERROR;
}
/**
* @brief Suspend Tick increment.
* @note Disable the tick increment by disabling TIM14 update interrupt.
* @param None
* @retval None
*/
void HAL_SuspendTick(void)
{
/* Disable TIM14 update Interrupt */
__HAL_TIM_DISABLE_IT(&htim14, TIM_IT_UPDATE);
}
/**
* @brief Resume Tick increment.
* @note Enable the tick increment by Enabling TIM14 update interrupt.
* @param None
* @retval None
*/
void HAL_ResumeTick(void)
{
/* Enable TIM14 Update interrupt */
__HAL_TIM_ENABLE_IT(&htim14, TIM_IT_UPDATE);
}
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Firmware/Src/stm32f4xx_it.c
+66 -15
View File
@@ -54,6 +54,12 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc2;
extern ADC_HandleTypeDef hadc3;
extern TIM_HandleTypeDef htim8;
extern DMA_HandleTypeDef hdma_uart4_rx;
extern DMA_HandleTypeDef hdma_uart4_tx;
extern UART_HandleTypeDef huart4;
extern TIM_HandleTypeDef htim14;
/******************************************************************************/
/* Cortex-M4 Processor Interruption and Exception Handlers */
@@ -157,7 +163,6 @@ 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 */
@@ -172,31 +177,31 @@ void SysTick_Handler(void)
/******************************************************************************/
/**
* @brief This function handles EXTI line0 interrupt.
* @brief This function handles DMA1 stream2 global interrupt.
*/
void EXTI0_IRQHandler(void)
void DMA1_Stream2_IRQHandler(void)
{
/* USER CODE BEGIN EXTI0_IRQn 0 */
/* USER CODE BEGIN DMA1_Stream2_IRQn 0 */
/* USER CODE END EXTI0_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0);
/* USER CODE BEGIN EXTI0_IRQn 1 */
/* USER CODE END DMA1_Stream2_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_uart4_rx);
/* USER CODE BEGIN DMA1_Stream2_IRQn 1 */
/* USER CODE END EXTI0_IRQn 1 */
/* USER CODE END DMA1_Stream2_IRQn 1 */
}
/**
* @brief This function handles EXTI line2 interrupt.
* @brief This function handles DMA1 stream4 global interrupt.
*/
void EXTI2_IRQHandler(void)
void DMA1_Stream4_IRQHandler(void)
{
/* USER CODE BEGIN EXTI2_IRQn 0 */
/* USER CODE BEGIN DMA1_Stream4_IRQn 0 */
/* USER CODE END EXTI2_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2);
/* USER CODE BEGIN EXTI2_IRQn 1 */
/* USER CODE END DMA1_Stream4_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_uart4_tx);
/* USER CODE BEGIN DMA1_Stream4_IRQn 1 */
/* USER CODE END EXTI2_IRQn 1 */
/* USER CODE END DMA1_Stream4_IRQn 1 */
}
/**
@@ -225,6 +230,35 @@ void ADC_IRQHandler(void)
/* USER CODE END ADC_IRQn 1 */
}
/**
* @brief This function handles TIM8 trigger and commutation interrupts and TIM14 global interrupt.
*/
void TIM8_TRG_COM_TIM14_IRQHandler(void)
{
/* USER CODE BEGIN TIM8_TRG_COM_TIM14_IRQn 0 */
/* USER CODE END TIM8_TRG_COM_TIM14_IRQn 0 */
HAL_TIM_IRQHandler(&htim8);
HAL_TIM_IRQHandler(&htim14);
/* USER CODE BEGIN TIM8_TRG_COM_TIM14_IRQn 1 */
/* USER CODE END TIM8_TRG_COM_TIM14_IRQn 1 */
}
/**
* @brief This function handles UART4 global interrupt.
*/
void UART4_IRQHandler(void)
{
/* USER CODE BEGIN UART4_IRQn 0 */
/* USER CODE END UART4_IRQn 0 */
HAL_UART_IRQHandler(&huart4);
/* USER CODE BEGIN UART4_IRQn 1 */
/* USER CODE END UART4_IRQn 1 */
}
/**
* @brief This function handles USB On The Go FS global interrupt.
*/
@@ -267,5 +301,22 @@ void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback) {
}
}
/**
* @brief This function handles EXTI line0 interrupt.
*/
void EXTI0_IRQHandler(void)
{
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0);
}
/**
* @brief This function handles EXTI line2 interrupt.
*/
void EXTI2_IRQHandler(void)
{
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2);
}
/* USER CODE END 1 */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Firmware/Src/syscalls.c
+34 -5
View File
@@ -6,7 +6,10 @@
*/
#include <sys/unistd.h>
#include "usbd_cdc_if.h"
#include <usbd_cdc_if.h>
#include <usart.h>
#include <commands.h>
//int _read(int file, char *data, int len) {}
//int _close(int file) {}
@@ -14,11 +17,37 @@
//int _fstat(int file, struct stat *st) {}
//int _isatty(int file) {}
#define UART_TX_BUFFER_SIZE 64
static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE];
int _write(int file, char *data, int len) {
//number of bytes written
int written = 0;
switch (serial_printf_select) {
// transmit over CDC
uint8_t status = CDC_Transmit_FS((uint8_t*)data, len);
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;
// return number of bytes written
return (status == USBD_OK ? len : 0);
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;
default: {
written = 0;
} break;
}
return written;
}
Firmware/Src/tim.c
+7
View File
@@ -373,6 +373,10 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* tim_pwmHandle)
/* USER CODE END TIM8_MspInit 0 */
/* TIM8 clock enable */
__HAL_RCC_TIM8_CLK_ENABLE();
/* TIM8 interrupt Init */
HAL_NVIC_SetPriority(TIM8_TRG_COM_TIM14_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TIM8_TRG_COM_TIM14_IRQn);
/* USER CODE BEGIN TIM8_MspInit 1 */
/* USER CODE END TIM8_MspInit 1 */
@@ -565,6 +569,9 @@ void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* tim_pwmHandle)
/* USER CODE END TIM8_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_TIM8_CLK_DISABLE();
/* TIM8 interrupt Deinit */
HAL_NVIC_DisableIRQ(TIM8_TRG_COM_TIM14_IRQn);
/* USER CODE BEGIN TIM8_MspDeInit 1 */
/* USER CODE END TIM8_MspDeInit 1 */
Firmware/Src/usart.c
+200
View File
@@ -0,0 +1,200 @@
/**
******************************************************************************
* File Name : USART.c
* Description : This file provides code for the configuration
* of the USART instances.
******************************************************************************
* This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* Copyright (c) 2017 STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted, provided that the following conditions are met:
*
* 1. Redistribution of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of other
* contributors to this software may be used to endorse or promote products
* derived from this software without specific written permission.
* 4. This software, including modifications and/or derivative works of this
* software, must execute solely and exclusively on microcontroller or
* microprocessor devices manufactured by or for STMicroelectronics.
* 5. Redistribution and use of this software other than as permitted under
* this license is void and will automatically terminate your rights under
* this license.
*
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "usart.h"
#include "gpio.h"
#include "dma.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
UART_HandleTypeDef huart4;
DMA_HandleTypeDef hdma_uart4_rx;
DMA_HandleTypeDef hdma_uart4_tx;
/* UART4 init function */
void MX_UART4_Init(void)
{
huart4.Instance = UART4;
huart4.Init.BaudRate = 115200;
huart4.Init.WordLength = UART_WORDLENGTH_8B;
huart4.Init.StopBits = UART_STOPBITS_1;
huart4.Init.Parity = UART_PARITY_NONE;
huart4.Init.Mode = UART_MODE_TX_RX;
huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart4.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart4) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
}
void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
{
GPIO_InitTypeDef GPIO_InitStruct;
if(uartHandle->Instance==UART4)
{
/* USER CODE BEGIN UART4_MspInit 0 */
/* USER CODE END UART4_MspInit 0 */
/* UART4 clock enable */
__HAL_RCC_UART4_CLK_ENABLE();
/**UART4 GPIO Configuration
PA0-WKUP ------> UART4_TX
PA1 ------> UART4_RX
*/
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
/* UART4 DMA Init */
/* UART4_RX Init */
hdma_uart4_rx.Instance = DMA1_Stream2;
hdma_uart4_rx.Init.Channel = DMA_CHANNEL_4;
hdma_uart4_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_uart4_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_uart4_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_uart4_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_uart4_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_uart4_rx.Init.Mode = DMA_CIRCULAR;
hdma_uart4_rx.Init.Priority = DMA_PRIORITY_LOW;
hdma_uart4_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_uart4_rx) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
__HAL_LINKDMA(uartHandle,hdmarx,hdma_uart4_rx);
/* UART4_TX Init */
hdma_uart4_tx.Instance = DMA1_Stream4;
hdma_uart4_tx.Init.Channel = DMA_CHANNEL_4;
hdma_uart4_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_uart4_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_uart4_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_uart4_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_uart4_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_uart4_tx.Init.Mode = DMA_NORMAL;
hdma_uart4_tx.Init.Priority = DMA_PRIORITY_LOW;
hdma_uart4_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_uart4_tx) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
__HAL_LINKDMA(uartHandle,hdmatx,hdma_uart4_tx);
/* UART4 interrupt Init */
HAL_NVIC_SetPriority(UART4_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(UART4_IRQn);
/* USER CODE BEGIN UART4_MspInit 1 */
/* USER CODE END UART4_MspInit 1 */
}
}
void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
{
if(uartHandle->Instance==UART4)
{
/* USER CODE BEGIN UART4_MspDeInit 0 */
/* USER CODE END UART4_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_UART4_CLK_DISABLE();
/**UART4 GPIO Configuration
PA0-WKUP ------> UART4_TX
PA1 ------> UART4_RX
*/
HAL_GPIO_DeInit(GPIOA, GPIO_1_Pin|GPIO_2_Pin);
/* UART4 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmarx);
HAL_DMA_DeInit(uartHandle->hdmatx);
/* UART4 interrupt Deinit */
HAL_NVIC_DisableIRQ(UART4_IRQn);
/* USER CODE BEGIN UART4_MspDeInit 1 */
/* USER CODE END UART4_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Firmware/Src/usb_device.c
+8 -1
View File
@@ -58,9 +58,13 @@
/* USB Device Core handle declaration */
USBD_HandleTypeDef hUsbDeviceFS;
/* init function */
/* init function */
void MX_USB_DEVICE_Init(void)
{
/* USER CODE BEGIN USB_DEVICE_Init_PreTreatment */
/* USER CODE END USB_DEVICE_Init_PreTreatment */
/* Init Device Library,Add Supported Class and Start the library*/
USBD_Init(&hUsbDeviceFS, &FS_Desc, DEVICE_FS);
@@ -70,6 +74,9 @@ void MX_USB_DEVICE_Init(void)
USBD_Start(&hUsbDeviceFS);
/* USER CODE BEGIN USB_DEVICE_Init_PostTreatment */
/* USER CODE END USB_DEVICE_Init_PostTreatment */
}
/**
* @}
Firmware/Src/usbd_cdc_if.c
+18 -11
View File
@@ -50,7 +50,7 @@
#include "usbd_cdc_if.h"
/* USER CODE BEGIN INCLUDE */
#include "utils.h"
#include "low_level.h"
#include "commands.h"
/* USER CODE END INCLUDE */
/** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
@@ -268,14 +268,14 @@ static int8_t CDC_Control_FS (uint8_t cmd, uint8_t* pbuf, uint16_t length)
static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
{
/* USER CODE BEGIN 6 */
USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &Buf[0]);
USBD_CDC_ReceivePacket(&hUsbDeviceFS);
//Append null termination at end of string
int null_idx = MACRO_MIN(*Len, APP_RX_DATA_SIZE-1);
Buf[null_idx] = 0;
int modified_len = MACRO_MIN(*Len+1, APP_RX_DATA_SIZE);
Buf[modified_len-1] = 0;
motor_parse_cmd(Buf, *Len);
motor_parse_cmd(Buf, modified_len, SERIAL_PRINTF_IS_USB);
// Allow next packet
USBD_CDC_ReceivePacket(&hUsbDeviceFS);
return (USBD_OK);
/* USER CODE END 6 */
@@ -296,11 +296,18 @@ uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len)
{
uint8_t result = USBD_OK;
/* USER CODE BEGIN 7 */
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*)hUsbDeviceFS.pClassData;
if (hcdc->TxState != 0){
//Check length
if (Len > APP_TX_DATA_SIZE)
return USBD_FAIL;
// Check for ongoing transmission
USBD_CDC_HandleTypeDef* hcdc = (USBD_CDC_HandleTypeDef*) hUsbDeviceFS.pClassData;
if (hcdc->TxState != 0)
return USBD_BUSY;
}
USBD_CDC_SetTxBuffer(&hUsbDeviceFS, Buf, Len);
// memcpy Buf into UserTxBufferFS
memcpy(UserTxBufferFS, Buf, Len);
// Update Len
USBD_CDC_SetTxBuffer(&hUsbDeviceFS, UserTxBufferFS, Len);
result = USBD_CDC_TransmitPacket(&hUsbDeviceFS);
/* USER CODE END 7 */
return result;
Firmware/Src/usbd_desc.c
+1 -1
View File
@@ -75,7 +75,7 @@
#define USBD_LANGID_STRING 1033
#define USBD_MANUFACTURER_STRING "ODrive"
#define USBD_PID_FS 0x0D31
#define USBD_PRODUCT_STRING_FS "ODrive rev 3.1"
#define USBD_PRODUCT_STRING_FS "ODrive v3.1"
#define USBD_SERIALNUMBER_STRING_FS "000000000001"
#define USBD_CONFIGURATION_STRING_FS "CDC Config"
#define USBD_INTERFACE_STRING_FS "CDC Interface"
Firmware/startup/startup_stm32f405xx.s
+3 -3
View File
@@ -2,8 +2,8 @@
******************************************************************************
* @file startup_stm32f405xx.s
* @author MCD Application Team
* @version V2.6.0
* @date 04-November-2016
* @version V2.6.1
* @date 14-February-2017
* @brief STM32F405xx Devices vector table for GCC based toolchains.
* This module performs:
* - Set the initial SP
@@ -16,7 +16,7 @@
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2016 STMicroelectronics</center></h2>
* <h2><center>&copy; COPYRIGHT 2017 STMicroelectronics</center></h2>
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
Firmware/tools/.gitignore
-1
View File
@@ -1 +0,0 @@
*.py[cod]
LICENSE
-21
View File
@@ -1,21 +0,0 @@
The MIT License (MIT)
Copyright (c) 2016 Oskar Weigl (madcowswe)
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
README.md
+1 -1
View File
@@ -35,7 +35,7 @@ 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)
### Sending commands
Sending USB and UART commands is documented [here].(Firmware/README.md#communicating-over-usb-and-uart)
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.
### Setting up UART
Baud rate: 115200
analysis/cogging_torque/cogging_harmonics.py
+52
View File
@@ -0,0 +1,52 @@
import numpy as np
import matplotlib.pyplot as plt
encoder_cpr = 2400
stator_slots = 12
pole_pairs = 7
N = data.size
fft = np.fft.rfft(data)
freq = np.fft.rfftfreq(N, d=1./encoder_cpr)
harmonics = [0]
harmonics += [(i+1)*stator_slots for i in range(pole_pairs)]
harmonics += [pole_pairs]
harmonics += [(i+1)*2*pole_pairs for i in range(int(stator_slots/4))]
fft_sparse = fft.copy()
indicies = np.arange(fft_sparse.size)
mask = [i not in harmonics for i in indicies]
fft_sparse[mask] = 0.0
interp_data = np.fft.irfft(fft_sparse)
#%%
#plt.figure()
plt.subplot(3, 1, 1)
plt.plot(data, label='raw')
plt.plot(interp_data, label='selected harmonics IFFT')
plt.title('cogging map')
plt.xlabel('counts')
plt.ylabel('A')
plt.legend(loc='best')
#plt.figure()
plt.subplot(3, 1, 2)
plt.stem(freq, np.abs(fft)/N, label='raw')
plt.stem(freq[harmonics], np.abs(fft_sparse[harmonics])/N, markerfmt='ro', label='selected harmonics')
plt.title('cogging map spectrum')
plt.xlabel('cycles/turn')
plt.ylabel('A')
plt.legend(loc='best')
#plt.figure()
plt.subplot(3, 1, 3)
plt.stem(freq, np.abs(fft)/N, label='raw')
plt.stem(freq[harmonics], np.abs(fft_sparse[harmonics])/N, markerfmt='ro', label='selected harmonics')
plt.title('cogging map spectrum')
plt.xlabel('cycles/turn')
plt.ylabel('A')
plt.legend(loc='best')
motor_analysis/350kvTP.PNG → analysis/motor_analysis/350kvTP.PNG
View File
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 13 KiB

After

Width:  |  Height:  |  Size: 13 KiB

motor_analysis/350kvVelli.PNG → analysis/motor_analysis/350kvVelli.PNG
View File
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 35 KiB

After

Width:  |  Height:  |  Size: 35 KiB

motor_analysis/VelliPlot.m → analysis/motor_analysis/VelliPlot.m
View File
numeric_path_opt/Main.m → analysis/numeric_path_opt/Main.m
View File
numeric_path_opt/predictionmatrices.m → analysis/numeric_path_opt/predictionmatrices.m
View File
Firmware/tools/odrive/__init__.py → tools/odrive/__init__.py
View File
Firmware/tools/odrive/usbbulk.py → tools/odrive/usbbulk.py
+3 -3
View File
@@ -108,9 +108,9 @@ class ODriveBulkDevice():
#return -1
raise
def recieve(self, bufferLen):
def receive(self, bufferLen):
try:
ret = self.epr.read(bufferLen, 0)
ret = self.epr.read(bufferLen, 100)
return ret
except usb.core.USBError:
#return -1
@@ -119,5 +119,5 @@ class ODriveBulkDevice():
def send_max(self):
return 64
def recieve_max(self):
def receive_max(self):
return 64
Firmware/tools/odrive/util.py → tools/odrive/util.py
View File
Firmware/tools/test_bulk.py → tools/test_communication.py
+11 -4
View File
@@ -26,28 +26,35 @@ def main(args):
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")
print("\tHALT:\n\t\th")
print("\tQuit Python Script:\n\t\tq")
print("---------------------------------------------------------------------")
# query device
dev = usbbulk.poll_odrive_bulk_device(printer=print)
print (dev.info())
print (dev.init())
# thread
thread = threading.Thread(target=recieve_thread, args=[dev])
thread = threading.Thread(target=receive_thread, args=[dev])
thread.start()
while running:
time.sleep(0.1)
try:
command = input("Enter ODrive command:\n")
dev.send(command)
if 'q' in command:
running = False
sys.exit()
else:
dev.send(command)
except:
running = False
def recieve_thread(dev):
def receive_thread(dev):
global ready
while running:
time.sleep(0.1)
try:
message = dev.recieve(dev.recieve_max())
message = dev.receive(dev.receive_max())
message_ascii = bytes(message).decode('ascii')
print(message_ascii, end='')
if "ODrive Firmware" in message_ascii:
various/path.c
-47
View File
@@ -1,47 +0,0 @@
#include <stdio.h>
#include <math.h>
void output(float x, float y, float str, float param) {
printf("{ %05.1ff, %05.1ff, %1.1ff, %06.1ff },\n", (x+1)*75, (y+1)*75, str, param);
}
void searchat(float x, float y) {
output(x, y, 0.4, 500);
//output(x+0.02, y, 0.4, 1000);
//output(x, y+0.02, 0.4, 1000);
//output(x-0.02, y, 0.4, 1000);
//output(x, y-0.02, 0.4, 1000);
}
void pickupto(float x, float y) {
output(x, y, 0.4, 0);
//output(x, y, 0.4, 1000);
output(x, y, 0, 1000);
}
void moveto(float x, float y) {
output(x, y, 0, 0);
}
#define POINTS 15
#define TWOPI (3.1415*2.0)
int main(int argc, char** argv) {
moveto(-1,-1);
for (int j=0; j<2; j++)
for (int i=0; i<POINTS; i++) {
float angle = ((float)i/POINTS + 0.5 + 0.125) * TWOPI ;
if (!(i%2) != !j)
pickupto(cos(angle)*0.8, sin(angle)*0.8);
else {
moveto(cos(angle)*0.8, sin(angle)*0.8);
searchat(cos(angle)*0.8, sin(angle)*0.8);
}
}
moveto(-1,-1);
}