Work with register
This commit is contained in:
commit
ba1ed0fd2b
35 changed files with 27811 additions and 0 deletions
1
.clang-tidy
Normal file
1
.clang-tidy
Normal file
|
|
@ -0,0 +1 @@
|
||||||
|
Checks: '-*, -misc-definitions-in-headers'
|
||||||
18
.clangd
Normal file
18
.clangd
Normal file
|
|
@ -0,0 +1,18 @@
|
||||||
|
CompileFlags:
|
||||||
|
Add:
|
||||||
|
[
|
||||||
|
# -mlong-calls,
|
||||||
|
-DSSIZE_MAX,
|
||||||
|
-DLWIP_NO_UNISTD_H=1,
|
||||||
|
-Dssize_t=long,
|
||||||
|
-D_SSIZE_T_DECLARED,
|
||||||
|
]
|
||||||
|
Remove:
|
||||||
|
[
|
||||||
|
-fno-tree-switch-conversion,
|
||||||
|
-mtext-section-literals,
|
||||||
|
-mlongcalls,
|
||||||
|
-fstrict-volatile-bitfields,
|
||||||
|
-free,
|
||||||
|
-fipa-pta,
|
||||||
|
]
|
||||||
9
.gitignore
vendored
Normal file
9
.gitignore
vendored
Normal file
|
|
@ -0,0 +1,9 @@
|
||||||
|
.pio
|
||||||
|
.vscode/.browse.c_cpp.db*
|
||||||
|
.vscode/c_cpp_properties.json
|
||||||
|
.vscode/launch.json
|
||||||
|
.vscode/ipch
|
||||||
|
.cache/
|
||||||
|
.metadata/
|
||||||
|
cubemx_config/
|
||||||
|
compile_commands.json
|
||||||
10
.vscode/extensions.json
vendored
Normal file
10
.vscode/extensions.json
vendored
Normal file
|
|
@ -0,0 +1,10 @@
|
||||||
|
{
|
||||||
|
// See http://go.microsoft.com/fwlink/?LinkId=827846
|
||||||
|
// for the documentation about the extensions.json format
|
||||||
|
"recommendations": [
|
||||||
|
"platformio.platformio-ide"
|
||||||
|
],
|
||||||
|
"unwantedRecommendations": [
|
||||||
|
"ms-vscode.cpptools-extension-pack"
|
||||||
|
]
|
||||||
|
}
|
||||||
70
README.md
Normal file
70
README.md
Normal file
|
|
@ -0,0 +1,70 @@
|
||||||
|
# Встроенное ПО для сервипривода на STM32F446RE
|
||||||
|
|
||||||
|
## Для разработки
|
||||||
|
|
||||||
|
- Установить platformio
|
||||||
|
|
||||||
|
```bash
|
||||||
|
pip install -U platformio
|
||||||
|
```
|
||||||
|
|
||||||
|
- Скомпилировать проект
|
||||||
|
|
||||||
|
```bash
|
||||||
|
platformio run --environment robotroller_reborn
|
||||||
|
```
|
||||||
|
|
||||||
|
- Загрузить прошивку
|
||||||
|
|
||||||
|
```bash
|
||||||
|
platformio run --target upload --environment robotroller_reborn
|
||||||
|
```
|
||||||
|
|
||||||
|
- Открыть монитор UART
|
||||||
|
|
||||||
|
```bash
|
||||||
|
platformio device monitor
|
||||||
|
```
|
||||||
|
|
||||||
|
## Загрузчик (Bootloader)
|
||||||
|
|
||||||
|
В директории `bootloader` расположен код загрузчика, который позволяет загрузить/обновить управляющую программу контроллера.
|
||||||
|
|
||||||
|
## Инструкция по загрузке ПО в контроллер по CAN-шине
|
||||||
|
|
||||||
|
1. Сместить в линкере прошивки её адрес. В файле с расширением .ld замените область FLASH на 0x08008000 и занимаемую память на 480K
|
||||||
|
пример (FLASH.ld и RAM.ld):
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
MEMORY
|
||||||
|
{
|
||||||
|
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K
|
||||||
|
FLASH (rx) : ORIGIN = 0x8008000, LENGTH = 480K
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
2. Cкомпилировать в `hex` формате
|
||||||
|
3. Запустить в терминале
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 boot_test.py <адрес на hex файл> <адрес устройства>
|
||||||
|
```
|
||||||
|
Пример:
|
||||||
|
```bash
|
||||||
|
python3 boot_test.py my_hex 0
|
||||||
|
```
|
||||||
|
4. После завершения процесса прошивки микроконтроллер автоматически перезагрузится и запустится загруженная программа, минуя загрузчик.
|
||||||
|
|
||||||
|
Бтулоадер запустится только в том случае, если по адресу 0x08060000 находится ключ DEADBEEF. При первой прошивке стартовый адрес = 0
|
||||||
|
|
||||||
|
## Запись CAN ID в контроллер
|
||||||
|
|
||||||
|
Запись происходит в основной прошщивке, вызывается регистр записи, после чего REG_ID c указанным адресом.
|
||||||
|
Пример:
|
||||||
|
```
|
||||||
|
msg_id: <адрес устройства>
|
||||||
|
CAN.buff[0] : REG_WRITE
|
||||||
|
Can.buff[1] : REG_ID
|
||||||
|
Can.buff[2] : <новый адрес устройства>
|
||||||
|
```
|
||||||
|
|
||||||
19
check_gcc_version.py
Normal file
19
check_gcc_version.py
Normal file
|
|
@ -0,0 +1,19 @@
|
||||||
|
Import("env")
|
||||||
|
|
||||||
|
# Получаем путь к компилятору из окружения PlatformIO
|
||||||
|
gcc_path = env.subst("$CC")
|
||||||
|
|
||||||
|
# Выполняем команду для получения версии компилятора
|
||||||
|
import subprocess
|
||||||
|
|
||||||
|
try:
|
||||||
|
result = subprocess.run([gcc_path, "--version"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True)
|
||||||
|
if result.returncode == 0:
|
||||||
|
print(f"GCC version: {result.stdout}")
|
||||||
|
else:
|
||||||
|
print(f"Failed to get GCC version: {result.stderr}")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"Error while getting GCC version: {e}")
|
||||||
|
|
||||||
|
# Дополнительно проверяем путь к компилятору
|
||||||
|
print(f"Compiler path: {gcc_path}")
|
||||||
9
compile_bootloader.py
Normal file
9
compile_bootloader.py
Normal file
|
|
@ -0,0 +1,9 @@
|
||||||
|
import os
|
||||||
|
Import("env")
|
||||||
|
|
||||||
|
|
||||||
|
# include toolchain paths
|
||||||
|
env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True)
|
||||||
|
|
||||||
|
# override compilation DB path
|
||||||
|
env.Replace(COMPILATIONDB_PATH="compile_commands.json")
|
||||||
299
cubemx_config.ioc
Normal file
299
cubemx_config.ioc
Normal file
|
|
@ -0,0 +1,299 @@
|
||||||
|
#MicroXplorer Configuration settings - do not modify
|
||||||
|
ADC2.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_15
|
||||||
|
ADC2.Channel-5\#ChannelRegularConversion=ADC_CHANNEL_8
|
||||||
|
ADC2.Channel-6\#ChannelRegularConversion=ADC_CHANNEL_9
|
||||||
|
ADC2.EOCSelection=ADC_EOC_SEQ_CONV
|
||||||
|
ADC2.IPParameters=Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,NbrOfConversionFlag,InjNumberOfConversion,NbrOfConversion,Rank-5\#ChannelRegularConversion,Channel-5\#ChannelRegularConversion,SamplingTime-5\#ChannelRegularConversion,Rank-6\#ChannelRegularConversion,Channel-6\#ChannelRegularConversion,SamplingTime-6\#ChannelRegularConversion,EOCSelection
|
||||||
|
ADC2.InjNumberOfConversion=0
|
||||||
|
ADC2.NbrOfConversion=3
|
||||||
|
ADC2.NbrOfConversionFlag=1
|
||||||
|
ADC2.Rank-1\#ChannelRegularConversion=1
|
||||||
|
ADC2.Rank-5\#ChannelRegularConversion=2
|
||||||
|
ADC2.Rank-6\#ChannelRegularConversion=3
|
||||||
|
ADC2.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
|
||||||
|
ADC2.SamplingTime-5\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
|
||||||
|
ADC2.SamplingTime-6\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
|
||||||
|
FREERTOS.IPParameters=Tasks01,configENABLE_FPU,configTIMER_TASK_PRIORITY
|
||||||
|
FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
|
||||||
|
FREERTOS.configENABLE_FPU=1
|
||||||
|
FREERTOS.configTIMER_TASK_PRIORITY=1
|
||||||
|
File.Version=6
|
||||||
|
GPIO.groupedBy=Group By Peripherals
|
||||||
|
KeepUserPlacement=false
|
||||||
|
Mcu.CPN=STM32F446RET6
|
||||||
|
Mcu.Family=STM32F4
|
||||||
|
Mcu.IP0=ADC2
|
||||||
|
Mcu.IP1=FREERTOS
|
||||||
|
Mcu.IP2=NVIC
|
||||||
|
Mcu.IP3=RCC
|
||||||
|
Mcu.IP4=SPI2
|
||||||
|
Mcu.IP5=SYS
|
||||||
|
Mcu.IP6=TIM1
|
||||||
|
Mcu.IP7=TIM3
|
||||||
|
Mcu.IP8=TIM5
|
||||||
|
Mcu.IP9=USART1
|
||||||
|
Mcu.IPNb=10
|
||||||
|
Mcu.Name=STM32F446R(C-E)Tx
|
||||||
|
Mcu.Package=LQFP64
|
||||||
|
Mcu.Pin0=PC1
|
||||||
|
Mcu.Pin1=PC5
|
||||||
|
Mcu.Pin10=PC9
|
||||||
|
Mcu.Pin11=PA8
|
||||||
|
Mcu.Pin12=PA9
|
||||||
|
Mcu.Pin13=PA10
|
||||||
|
Mcu.Pin14=PA11
|
||||||
|
Mcu.Pin15=PA12
|
||||||
|
Mcu.Pin16=PA13
|
||||||
|
Mcu.Pin17=PA14
|
||||||
|
Mcu.Pin18=PC10
|
||||||
|
Mcu.Pin19=PC11
|
||||||
|
Mcu.Pin2=PB0
|
||||||
|
Mcu.Pin20=PC12
|
||||||
|
Mcu.Pin21=PD2
|
||||||
|
Mcu.Pin22=PB6
|
||||||
|
Mcu.Pin23=PB7
|
||||||
|
Mcu.Pin24=VP_FREERTOS_VS_CMSIS_V2
|
||||||
|
Mcu.Pin25=VP_SYS_VS_tim2
|
||||||
|
Mcu.Pin26=VP_TIM1_VS_ClockSourceINT
|
||||||
|
Mcu.Pin27=VP_TIM3_VS_ClockSourceINT
|
||||||
|
Mcu.Pin28=VP_TIM5_VS_ClockSourceINT
|
||||||
|
Mcu.Pin3=PB1
|
||||||
|
Mcu.Pin4=PB10
|
||||||
|
Mcu.Pin5=PB14
|
||||||
|
Mcu.Pin6=PB15
|
||||||
|
Mcu.Pin7=PC6
|
||||||
|
Mcu.Pin8=PC7
|
||||||
|
Mcu.Pin9=PC8
|
||||||
|
Mcu.PinsNb=29
|
||||||
|
Mcu.ThirdPartyNb=0
|
||||||
|
Mcu.UserConstants=
|
||||||
|
Mcu.UserName=STM32F446RETx
|
||||||
|
MxCube.Version=6.5.0
|
||||||
|
MxDb.Version=DB.6.0.50
|
||||||
|
NVIC.ADC_IRQn=true\:5\:0\:true\:true\:true\:1\:true\:true\:true\:true
|
||||||
|
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||||
|
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||||
|
NVIC.ForceEnableDMAVector=true
|
||||||
|
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||||
|
NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||||
|
NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||||
|
NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:false\:false
|
||||||
|
NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
|
||||||
|
NVIC.SPI2_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||||
|
NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false\:false
|
||||||
|
NVIC.SavedPendsvIrqHandlerGenerated=true
|
||||||
|
NVIC.SavedSvcallIrqHandlerGenerated=true
|
||||||
|
NVIC.SavedSystickIrqHandlerGenerated=true
|
||||||
|
NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false
|
||||||
|
NVIC.TIM2_IRQn=true\:15\:0\:true\:false\:true\:false\:false\:true\:true
|
||||||
|
NVIC.TIM3_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||||
|
NVIC.TimeBase=TIM2_IRQn
|
||||||
|
NVIC.TimeBaseIP=TIM2
|
||||||
|
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||||
|
PA10.Signal=S_TIM1_CH3
|
||||||
|
PA11.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
|
||||||
|
PA11.GPIO_Label=EN_U
|
||||||
|
PA11.GPIO_PuPd=GPIO_PULLDOWN
|
||||||
|
PA11.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
|
||||||
|
PA11.Locked=true
|
||||||
|
PA11.Signal=GPIO_Output
|
||||||
|
PA12.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
|
||||||
|
PA12.GPIO_Label=EN_V
|
||||||
|
PA12.GPIO_PuPd=GPIO_PULLDOWN
|
||||||
|
PA12.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
|
||||||
|
PA12.Locked=true
|
||||||
|
PA12.Signal=GPIO_Output
|
||||||
|
PA13.Mode=Serial_Wire
|
||||||
|
PA13.Signal=SYS_JTMS-SWDIO
|
||||||
|
PA14.Mode=Serial_Wire
|
||||||
|
PA14.Signal=SYS_JTCK-SWCLK
|
||||||
|
PA8.Signal=S_TIM1_CH1
|
||||||
|
PA9.Signal=S_TIM1_CH2
|
||||||
|
PB0.GPIOParameters=GPIO_Label
|
||||||
|
PB0.GPIO_Label=SENSE2
|
||||||
|
PB0.Locked=true
|
||||||
|
PB0.Signal=ADCx_IN8
|
||||||
|
PB1.GPIOParameters=GPIO_Label
|
||||||
|
PB1.GPIO_Label=SENSE1
|
||||||
|
PB1.Locked=true
|
||||||
|
PB1.Signal=ADCx_IN9
|
||||||
|
PB10.Locked=true
|
||||||
|
PB10.Mode=Full_Duplex_Master
|
||||||
|
PB10.Signal=SPI2_SCK
|
||||||
|
PB14.Locked=true
|
||||||
|
PB14.Mode=Full_Duplex_Master
|
||||||
|
PB14.Signal=SPI2_MISO
|
||||||
|
PB15.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label
|
||||||
|
PB15.GPIO_Label=AS5045_CS
|
||||||
|
PB15.GPIO_PuPd=GPIO_PULLUP
|
||||||
|
PB15.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
|
||||||
|
PB15.Locked=true
|
||||||
|
PB15.PinState=GPIO_PIN_SET
|
||||||
|
PB15.Signal=GPIO_Output
|
||||||
|
PB6.Mode=Asynchronous
|
||||||
|
PB6.Signal=USART1_TX
|
||||||
|
PB7.Mode=Asynchronous
|
||||||
|
PB7.Signal=USART1_RX
|
||||||
|
PC1.Mode=Full_Duplex_Master
|
||||||
|
PC1.Signal=SPI2_MOSI
|
||||||
|
PC10.GPIOParameters=GPIO_Label
|
||||||
|
PC10.GPIO_Label=LED1
|
||||||
|
PC10.Locked=true
|
||||||
|
PC10.Signal=GPIO_Output
|
||||||
|
PC11.GPIOParameters=GPIO_Label
|
||||||
|
PC11.GPIO_Label=LED2
|
||||||
|
PC11.Locked=true
|
||||||
|
PC11.Signal=GPIO_Output
|
||||||
|
PC12.GPIOParameters=GPIO_Label
|
||||||
|
PC12.GPIO_Label=LED3
|
||||||
|
PC12.Locked=true
|
||||||
|
PC12.Signal=GPIO_Output
|
||||||
|
PC5.GPIOParameters=GPIO_Label
|
||||||
|
PC5.GPIO_Label=SENSE3
|
||||||
|
PC5.Locked=true
|
||||||
|
PC5.Signal=ADCx_IN15
|
||||||
|
PC6.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
|
||||||
|
PC6.GPIO_Label=EN_W
|
||||||
|
PC6.GPIO_PuPd=GPIO_PULLDOWN
|
||||||
|
PC6.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
|
||||||
|
PC6.Locked=true
|
||||||
|
PC6.Signal=GPIO_Output
|
||||||
|
PC7.GPIOParameters=GPIO_Label
|
||||||
|
PC7.GPIO_Label=DRV_FAULT
|
||||||
|
PC7.Locked=true
|
||||||
|
PC7.Signal=GPIO_Input
|
||||||
|
PC8.GPIOParameters=GPIO_Label
|
||||||
|
PC8.GPIO_Label=DRV_RESET
|
||||||
|
PC8.Locked=true
|
||||||
|
PC8.Signal=GPIO_Output
|
||||||
|
PC9.GPIOParameters=GPIO_Label
|
||||||
|
PC9.GPIO_Label=DRV_SLEEP
|
||||||
|
PC9.Locked=true
|
||||||
|
PC9.Signal=GPIO_Output
|
||||||
|
PD2.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
|
||||||
|
PD2.GPIO_Label=spi1_cs
|
||||||
|
PD2.GPIO_PuPd=GPIO_PULLDOWN
|
||||||
|
PD2.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
|
||||||
|
PD2.Locked=true
|
||||||
|
PD2.Signal=GPIO_Output
|
||||||
|
PinOutPanel.RotationAngle=0
|
||||||
|
ProjectManager.AskForMigrate=true
|
||||||
|
ProjectManager.BackupPrevious=false
|
||||||
|
ProjectManager.CompilerOptimize=6
|
||||||
|
ProjectManager.ComputerToolchain=false
|
||||||
|
ProjectManager.CoupleFile=true
|
||||||
|
ProjectManager.CustomerFirmwarePackage=
|
||||||
|
ProjectManager.DefaultFWLocation=true
|
||||||
|
ProjectManager.DeletePrevious=true
|
||||||
|
ProjectManager.DeviceId=STM32F446RETx
|
||||||
|
ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.27.1
|
||||||
|
ProjectManager.FreePins=false
|
||||||
|
ProjectManager.HalAssertFull=false
|
||||||
|
ProjectManager.HeapSize=0x200
|
||||||
|
ProjectManager.KeepUserCode=true
|
||||||
|
ProjectManager.LastFirmware=true
|
||||||
|
ProjectManager.LibraryCopy=1
|
||||||
|
ProjectManager.MainLocation=Src
|
||||||
|
ProjectManager.NoMain=false
|
||||||
|
ProjectManager.PreviousToolchain=STM32CubeIDE
|
||||||
|
ProjectManager.ProjectBuild=false
|
||||||
|
ProjectManager.ProjectFileName=cubemx_config.ioc
|
||||||
|
ProjectManager.ProjectName=cubemx_config
|
||||||
|
ProjectManager.RegisterCallBack=
|
||||||
|
ProjectManager.StackSize=0x400
|
||||||
|
ProjectManager.TargetToolchain=Other Toolchains (GPDSC)
|
||||||
|
ProjectManager.ToolChainLocation=
|
||||||
|
ProjectManager.UnderRoot=false
|
||||||
|
ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-SystemClock_Config-RCC-false-HAL-false,3-MX_TIM1_Init-TIM1-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-HAL-true,5-MX_SPI2_Init-SPI2-false-HAL-true,6-MX_TIM3_Init-TIM3-false-HAL-true,7-MX_ADC2_Init-ADC2-false-HAL-true,8-MX_TIM5_Init-TIM5-false-HAL-true
|
||||||
|
RCC.AHBFreq_Value=180000000
|
||||||
|
RCC.APB1CLKDivider=RCC_HCLK_DIV4
|
||||||
|
RCC.APB1Freq_Value=45000000
|
||||||
|
RCC.APB1TimFreq_Value=90000000
|
||||||
|
RCC.APB2CLKDivider=RCC_HCLK_DIV2
|
||||||
|
RCC.APB2Freq_Value=90000000
|
||||||
|
RCC.APB2TimFreq_Value=180000000
|
||||||
|
RCC.CECFreq_Value=32786.88524590164
|
||||||
|
RCC.CortexFreq_Value=180000000
|
||||||
|
RCC.FCLKCortexFreq_Value=180000000
|
||||||
|
RCC.FMPI2C1Freq_Value=45000000
|
||||||
|
RCC.FamilyName=M
|
||||||
|
RCC.HCLKFreq_Value=180000000
|
||||||
|
RCC.HSE_VALUE=8000000
|
||||||
|
RCC.I2S1Freq_Value=96000000
|
||||||
|
RCC.I2S2Freq_Value=96000000
|
||||||
|
RCC.IPParameters=AHBFreq_Value,APB1CLKDivider,APB1Freq_Value,APB1TimFreq_Value,APB2CLKDivider,APB2Freq_Value,APB2TimFreq_Value,CECFreq_Value,CortexFreq_Value,FCLKCortexFreq_Value,FMPI2C1Freq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,I2S1Freq_Value,I2S2Freq_Value,MCO2PinFreq_Value,PLLCLKFreq_Value,PLLI2SPCLKFreq_Value,PLLI2SQCLKFreq_Value,PLLI2SRCLKFreq_Value,PLLI2SoutputFreq_Value,PLLM,PLLN,PLLQCLKFreq_Value,PLLRCLKFreq_Value,PLLSAIPCLKFreq_Value,PLLSAIQCLKFreq_Value,PLLSAIoutputFreq_Value,PWRFreq_Value,SAIAFreq_Value,SAIBFreq_Value,SDIOFreq_Value,SPDIFRXFreq_Value,SYSCLKFreq_VALUE,SYSCLKSource,USBFreq_Value,VCOI2SInputFreq_Value,VCOI2SOutputFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VCOSAIInputFreq_Value,VCOSAIOutputFreq_Value
|
||||||
|
RCC.MCO2PinFreq_Value=180000000
|
||||||
|
RCC.PLLCLKFreq_Value=180000000
|
||||||
|
RCC.PLLI2SPCLKFreq_Value=96000000
|
||||||
|
RCC.PLLI2SQCLKFreq_Value=96000000
|
||||||
|
RCC.PLLI2SRCLKFreq_Value=96000000
|
||||||
|
RCC.PLLI2SoutputFreq_Value=96000000
|
||||||
|
RCC.PLLM=8
|
||||||
|
RCC.PLLN=180
|
||||||
|
RCC.PLLQCLKFreq_Value=180000000
|
||||||
|
RCC.PLLRCLKFreq_Value=180000000
|
||||||
|
RCC.PLLSAIPCLKFreq_Value=96000000
|
||||||
|
RCC.PLLSAIQCLKFreq_Value=96000000
|
||||||
|
RCC.PLLSAIoutputFreq_Value=96000000
|
||||||
|
RCC.PWRFreq_Value=180000000
|
||||||
|
RCC.SAIAFreq_Value=96000000
|
||||||
|
RCC.SAIBFreq_Value=96000000
|
||||||
|
RCC.SDIOFreq_Value=180000000
|
||||||
|
RCC.SPDIFRXFreq_Value=180000000
|
||||||
|
RCC.SYSCLKFreq_VALUE=180000000
|
||||||
|
RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK
|
||||||
|
RCC.USBFreq_Value=180000000
|
||||||
|
RCC.VCOI2SInputFreq_Value=1000000
|
||||||
|
RCC.VCOI2SOutputFreq_Value=192000000
|
||||||
|
RCC.VCOInputFreq_Value=2000000
|
||||||
|
RCC.VCOOutputFreq_Value=360000000
|
||||||
|
RCC.VCOSAIInputFreq_Value=1000000
|
||||||
|
RCC.VCOSAIOutputFreq_Value=192000000
|
||||||
|
SH.ADCx_IN15.0=ADC2_IN15,IN15
|
||||||
|
SH.ADCx_IN15.ConfNb=1
|
||||||
|
SH.ADCx_IN8.0=ADC2_IN8,IN8
|
||||||
|
SH.ADCx_IN8.ConfNb=1
|
||||||
|
SH.ADCx_IN9.0=ADC2_IN9,IN9
|
||||||
|
SH.ADCx_IN9.ConfNb=1
|
||||||
|
SH.S_TIM1_CH1.0=TIM1_CH1,PWM Generation1 CH1
|
||||||
|
SH.S_TIM1_CH1.ConfNb=1
|
||||||
|
SH.S_TIM1_CH2.0=TIM1_CH2,PWM Generation2 CH2
|
||||||
|
SH.S_TIM1_CH2.ConfNb=1
|
||||||
|
SH.S_TIM1_CH3.0=TIM1_CH3,PWM Generation3 CH3
|
||||||
|
SH.S_TIM1_CH3.ConfNb=1
|
||||||
|
SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_64
|
||||||
|
SPI2.CLKPhase=SPI_PHASE_1EDGE
|
||||||
|
SPI2.CLKPolarity=SPI_POLARITY_LOW
|
||||||
|
SPI2.CalculateBaudRate=703.125 KBits/s
|
||||||
|
SPI2.DataSize=SPI_DATASIZE_16BIT
|
||||||
|
SPI2.Direction=SPI_DIRECTION_2LINES
|
||||||
|
SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,CLKPhase,BaudRatePrescaler,CLKPolarity
|
||||||
|
SPI2.Mode=SPI_MODE_MASTER
|
||||||
|
SPI2.VirtualType=VM_MASTER
|
||||||
|
TIM1.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
|
||||||
|
TIM1.BreakState=TIM_BREAK_DISABLE
|
||||||
|
TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
|
||||||
|
TIM1.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
|
||||||
|
TIM1.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
|
||||||
|
TIM1.CounterMode=TIM_COUNTERMODE_CENTERALIGNED1
|
||||||
|
TIM1.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,TIM_MasterOutputTrigger,AutoReloadPreload,BreakState,OffStateRunMode,OffStateIDLEMode,CounterMode,Period
|
||||||
|
TIM1.OffStateIDLEMode=TIM_OSSI_DISABLE
|
||||||
|
TIM1.OffStateRunMode=TIM_OSSR_DISABLE
|
||||||
|
TIM1.Period=2399
|
||||||
|
TIM1.TIM_MasterOutputTrigger=TIM_TRGO_RESET
|
||||||
|
TIM3.IPParameters=Period,Prescaler
|
||||||
|
TIM3.Period=99
|
||||||
|
TIM3.Prescaler=89
|
||||||
|
USART1.IPParameters=VirtualMode
|
||||||
|
USART1.VirtualMode=VM_ASYNC
|
||||||
|
VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
|
||||||
|
VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
|
||||||
|
VP_SYS_VS_tim2.Mode=TIM2
|
||||||
|
VP_SYS_VS_tim2.Signal=SYS_VS_tim2
|
||||||
|
VP_TIM1_VS_ClockSourceINT.Mode=Internal
|
||||||
|
VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT
|
||||||
|
VP_TIM3_VS_ClockSourceINT.Mode=Internal
|
||||||
|
VP_TIM3_VS_ClockSourceINT.Signal=TIM3_VS_ClockSourceINT
|
||||||
|
VP_TIM5_VS_ClockSourceINT.Mode=Internal
|
||||||
|
VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT
|
||||||
|
board=custom
|
||||||
20
gen_compile_commands.py
Normal file
20
gen_compile_commands.py
Normal file
|
|
@ -0,0 +1,20 @@
|
||||||
|
import os
|
||||||
|
Import("env")
|
||||||
|
|
||||||
|
|
||||||
|
# Custom HEX from ELF
|
||||||
|
env.AddPostAction(
|
||||||
|
"$BUILD_DIR/${PROGNAME}.elf",
|
||||||
|
env.VerboseAction(" ".join([
|
||||||
|
"$OBJCOPY", "-O", "ihex", "-R", ".eeprom",
|
||||||
|
"$BUILD_DIR/${PROGNAME}.elf", "$BUILD_DIR/${PROGNAME}.hex"
|
||||||
|
]), "Building $BUILD_DIR/${PROGNAME}.hex")
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# include toolchain paths
|
||||||
|
env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True)
|
||||||
|
|
||||||
|
# override compilation DB path
|
||||||
|
env.Replace(COMPILATIONDB_PATH="compile_commands.json")
|
||||||
|
|
||||||
72
include/flash.h
Normal file
72
include/flash.h
Normal file
|
|
@ -0,0 +1,72 @@
|
||||||
|
#ifndef FLASH_H_
|
||||||
|
#define FLASH_H_
|
||||||
|
#include "stm32f446xx.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* for addr in FLASH */
|
||||||
|
typedef struct{
|
||||||
|
uint8_t data_id; // data_id = id register of can
|
||||||
|
uint8_t value;
|
||||||
|
uint16_t crc;
|
||||||
|
// uint32_t write_ptr_now;
|
||||||
|
}FLASH_RECORD;
|
||||||
|
enum {
|
||||||
|
addr_id = 0,
|
||||||
|
foc_id = 1,
|
||||||
|
kp_id = 2,
|
||||||
|
ki_id = 3,
|
||||||
|
kd_id = 4
|
||||||
|
};
|
||||||
|
|
||||||
|
#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct
|
||||||
|
#define PARAM_COUNT 3 // count data in flash
|
||||||
|
|
||||||
|
// Flash sectors for STM32F407
|
||||||
|
|
||||||
|
#define SECTOR_2 0x08008000 // 16KB
|
||||||
|
#define SECTOR_3 0x0800C000 // 16KB
|
||||||
|
#define SECTOR_4 0x08010000 // 64KB
|
||||||
|
#define SECTOR_5 0x08020000 // 128KB
|
||||||
|
#define SECTOR_6 0x08040000 // 128KB
|
||||||
|
#define SECTOR_6_END (SECTOR_6 + 128 * 1024) // sector 6 end
|
||||||
|
#define SECTOR_7 0x08060000 // 128KB
|
||||||
|
|
||||||
|
|
||||||
|
#define FLAG_BOOT (0x08040000 + 4)
|
||||||
|
// Flash keys for unlocking flash memory
|
||||||
|
#define BYTE32 0
|
||||||
|
#define BYTE8 1
|
||||||
|
#define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение
|
||||||
|
//FLASH SET ONE PROGRAMM WORD
|
||||||
|
#define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1
|
||||||
|
#define FLASH_32BYTE \
|
||||||
|
FLASH->CR = (FLASH->CR & ~FLASH_CR_PSIZE) | (0x2 << FLASH_CR_PSIZE_Pos)
|
||||||
|
|
||||||
|
// Flash command bits
|
||||||
|
#define FLASH_LOCK FLASH->CR |= FLASH_CR_LOCK
|
||||||
|
#define FLASH_UNLOCK FLASH->KEYR = FLASH_KEY1; FLASH->KEYR = FLASH_KEY2
|
||||||
|
|
||||||
|
|
||||||
|
// Flash status flags
|
||||||
|
#define FLASH_BUSY (FLASH->SR & FLASH_SR_BSY)
|
||||||
|
#define FLASH_ERROR (FLASH->SR & (FLASH_SR_WRPERR | FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR))
|
||||||
|
|
||||||
|
//for bootloader
|
||||||
|
typedef void(*pFunction)(void);
|
||||||
|
|
||||||
|
// Function prototypes
|
||||||
|
void flash_unlock(void);
|
||||||
|
void flash_lock(void);
|
||||||
|
void erase_sector(uint8_t sector);
|
||||||
|
void flash_program_word(uint32_t address, uint32_t data,uint32_t byte_len);
|
||||||
|
uint8_t flash_read_word(uint32_t address);
|
||||||
|
void write_param(uint8_t param_id, uint8_t val);
|
||||||
|
FLASH_RECORD* load_params();
|
||||||
|
void compact_page();
|
||||||
|
void flash_read(uint32_t addr,FLASH_RECORD* ptr);
|
||||||
|
bool validate_crc(FLASH_RECORD* crc);
|
||||||
|
void flash_write(uint32_t addr, FLASH_RECORD* record);
|
||||||
|
|
||||||
|
#endif /* FLASH_H_ */
|
||||||
38
include/hal_conf_extra.h
Normal file
38
include/hal_conf_extra.h
Normal file
|
|
@ -0,0 +1,38 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#pragma region "Motor and sensor setup"
|
||||||
|
#define LED1 PC10
|
||||||
|
#define LED2 PC11
|
||||||
|
#define HARDWARE_SERIAL_RX_PIN PB7
|
||||||
|
#define HARDWARE_SERIAL_TX_PIN PB6
|
||||||
|
#define AS5045_CS PB15
|
||||||
|
#define AS5045_MISO PB14
|
||||||
|
#define AS5045_MOSI PC1
|
||||||
|
#define AS5045_SCLK PB10
|
||||||
|
#define CURRENT_SENSOR_1 PB1
|
||||||
|
#define CURRENT_SENSOR_2 PB0
|
||||||
|
#define CURRENT_SENSOR_3 PC5
|
||||||
|
#define TIM1_CH1 PA8
|
||||||
|
#define TIM1_CH2 PA9
|
||||||
|
#define TIM1_CH3 PA10
|
||||||
|
#define EN_W_GATE_DRIVER PC6
|
||||||
|
#define EN_U_GATE_DRIVER PA11
|
||||||
|
#define EN_V_GATE_DRIVER PA12
|
||||||
|
#define SLEEP_DRIVER PC9
|
||||||
|
#define RESET_DRIVER PC8
|
||||||
|
#define FAULT_DRIVER PC7
|
||||||
|
#define POLE_PAIRS 14
|
||||||
|
#define CAN2_TX PB13
|
||||||
|
#define CAN2_RX PB12
|
||||||
|
#define CAN1_TX PB9
|
||||||
|
#define CAN1_RX PB8
|
||||||
|
#define GM6208_RESISTANCE 31
|
||||||
|
#define OWN_RESISTANCE 26
|
||||||
|
#pragma endregion
|
||||||
|
|
||||||
|
#if !defined(HAL_CAN_MODULE_ENABLED)
|
||||||
|
#define HAL_CAN_MODULE_ENABLED
|
||||||
|
#endif
|
||||||
|
#include "stm32f4xx_hal.h"
|
||||||
|
#include "stm32f4xx_hal_can.h"
|
||||||
|
#include <STM32_CAN.h>
|
||||||
40
include/reg_cah.h
Normal file
40
include/reg_cah.h
Normal file
|
|
@ -0,0 +1,40 @@
|
||||||
|
#ifndef REG_CAH_H_
|
||||||
|
#define REG_CAH_H_
|
||||||
|
|
||||||
|
#define APP_ADDR 0x0800400 // 16KB - Application
|
||||||
|
#define ADDR_VAR 0x8040000
|
||||||
|
|
||||||
|
|
||||||
|
#define REG_READ 0x07
|
||||||
|
#define REG_WRITE 0x08
|
||||||
|
|
||||||
|
|
||||||
|
/* Startup ID device */
|
||||||
|
#define START_ID 0x00
|
||||||
|
|
||||||
|
/* CAN REGISTER ID */
|
||||||
|
#define REG_ID 0x01
|
||||||
|
#define REG_BAUDRATE 0x02
|
||||||
|
|
||||||
|
#define REG_MOTOR_POSPID_Kp 0x30
|
||||||
|
#define REG_MOTOR_POSPID_Ki 0x31
|
||||||
|
#define REG_MOTOR_POSPID_Kd 0x32
|
||||||
|
|
||||||
|
#define REG_MOTOR_VELPID_Kp 0x40
|
||||||
|
#define REG_MOTOR_VELPID_Ki 0x41
|
||||||
|
#define REG_MOTOR_VELPID_Kd 0x42
|
||||||
|
|
||||||
|
#define REG_MOTOR_IMPPID_Kp 0x50
|
||||||
|
#define REG_MOTOR_IMPPID_Kd 0x51
|
||||||
|
|
||||||
|
#define REG_RESET 0x88
|
||||||
|
#define REG_LED_BLINK 0x8B
|
||||||
|
|
||||||
|
#define FOC_STATE 0x60
|
||||||
|
|
||||||
|
#define MOTOR_VELOCITY 0x70
|
||||||
|
#define MOTOR_ENABLED 0x71
|
||||||
|
#define MOTOR_ANGLE 0x72
|
||||||
|
#define MOTOR_TORQUE 0x73
|
||||||
|
|
||||||
|
#endif // REG_CAH_H_
|
||||||
25781
knowledges/AS5045B_DS000397_2-00.pdf
Normal file
25781
knowledges/AS5045B_DS000397_2-00.pdf
Normal file
File diff suppressed because one or more lines are too long
BIN
knowledges/TI-DRV8313.pdf
Normal file
BIN
knowledges/TI-DRV8313.pdf
Normal file
Binary file not shown.
BIN
knowledges/motorBoard.pdf
Normal file
BIN
knowledges/motorBoard.pdf
Normal file
Binary file not shown.
Binary file not shown.
44
lib/AS5045/AS5045.cpp
Normal file
44
lib/AS5045/AS5045.cpp
Normal file
|
|
@ -0,0 +1,44 @@
|
||||||
|
#include "AS5045.h"
|
||||||
|
|
||||||
|
|
||||||
|
MagneticSensorAS5045::MagneticSensorAS5045(uint16_t as5040_cs, uint16_t as5040_mosi, uint16_t as5040_miso,
|
||||||
|
uint16_t as5040_sclk): AS5040_CS_(as5040_cs),
|
||||||
|
AS5040_MOSI_(as5040_mosi),
|
||||||
|
AS5040_MISO_(as5040_miso),
|
||||||
|
AS5040_SCLK_(as5040_sclk),
|
||||||
|
spi(nullptr),
|
||||||
|
settings(AS5145SSISettings) {
|
||||||
|
}
|
||||||
|
|
||||||
|
MagneticSensorAS5045::~MagneticSensorAS5045() = default;
|
||||||
|
|
||||||
|
auto MagneticSensorAS5045::init(SPIClass *_spi) -> void {
|
||||||
|
this->spi = _spi;
|
||||||
|
settings = AS5145SSISettings;
|
||||||
|
pinMode(AS5040_CS_, OUTPUT);
|
||||||
|
pinMode(AS5040_MISO_, INPUT);
|
||||||
|
pinMode(AS5040_MOSI_, OUTPUT);
|
||||||
|
pinMode(AS5040_SCLK_, OUTPUT);
|
||||||
|
|
||||||
|
spi->setMISO(AS5040_MISO_);
|
||||||
|
spi->setMOSI(AS5040_MOSI_);
|
||||||
|
spi->setSCLK(AS5040_SCLK_);
|
||||||
|
spi->begin();
|
||||||
|
this->Sensor::init();
|
||||||
|
}
|
||||||
|
|
||||||
|
float MagneticSensorAS5045::getSensorAngle() {
|
||||||
|
float angle_data = readRawAngleSSI();
|
||||||
|
angle_data = (static_cast<float>(angle_data) / AS5045_CPR) * _2PI;
|
||||||
|
return angle_data;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t MagneticSensorAS5045::readRawAngleSSI() const {
|
||||||
|
spi->beginTransaction(settings);
|
||||||
|
digitalWrite(AS5040_CS_, LOW);
|
||||||
|
uint16_t value = spi->transfer16(0x0000);
|
||||||
|
digitalWrite(AS5040_CS_, HIGH);
|
||||||
|
spi->endTransaction();
|
||||||
|
delayMicroseconds(30);
|
||||||
|
return (value >> 3) & 0x1FFF; // TODO(vanyabeat): Add error checking MAGNETIC OWERFLOW and etc.
|
||||||
|
}
|
||||||
32
lib/AS5045/AS5045.h
Normal file
32
lib/AS5045/AS5045.h
Normal file
|
|
@ -0,0 +1,32 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "SimpleFOC.h"
|
||||||
|
#include "SPI.h"
|
||||||
|
|
||||||
|
#ifndef MSBFIRST
|
||||||
|
#define MSBFIRST BitOrder::MSBFIRST
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define AS5045_BITORDER MSBFIRST
|
||||||
|
#define AS5045_CPR 4096.0f
|
||||||
|
#define _2PI 6.28318530718f
|
||||||
|
|
||||||
|
static SPISettings AS5145SSISettings(1000000, AS5045_BITORDER, SPI_MODE0);
|
||||||
|
|
||||||
|
class MagneticSensorAS5045 final: public Sensor {
|
||||||
|
public:
|
||||||
|
MagneticSensorAS5045(uint16_t as5040_cs, uint16_t as5040_mosi, uint16_t as5040_miso, uint16_t as5040_sclk);
|
||||||
|
|
||||||
|
virtual ~MagneticSensorAS5045();
|
||||||
|
|
||||||
|
float getSensorAngle() override;
|
||||||
|
|
||||||
|
virtual void init(SPIClass *_spi = &SPI);
|
||||||
|
|
||||||
|
[[nodiscard]] uint16_t readRawAngleSSI() const;
|
||||||
|
|
||||||
|
private:
|
||||||
|
uint16_t AS5040_CS_, AS5040_MOSI_, AS5040_MISO_, AS5040_SCLK_;
|
||||||
|
SPIClass *spi;
|
||||||
|
SPISettings settings;
|
||||||
|
};
|
||||||
10
lib/AS5045/library.properties
Normal file
10
lib/AS5045/library.properties
Normal file
|
|
@ -0,0 +1,10 @@
|
||||||
|
name=AS5045
|
||||||
|
version=1.0.1
|
||||||
|
author=vanyabeat <vanyabeat@protonmail.com>
|
||||||
|
maintainer=vanyabeat <vanyabeat@protonmail.com>
|
||||||
|
sentence=Simple library to work with AS5040 and Simple FOC (for Robotroller in Robosemmbler) Fork from https://github.com/runger1101001
|
||||||
|
paragraph=Simple library to work with AS5040 and Simple FOC and simple library intended for hobby comunity to run the AS5040 magnetic sensor using FOC algorithm. It is intended to support as many BLDC/Stepper motor+sensor+driver combinations as possible and in the same time maintain simplicity of usage. Library supports Arudino devices such as Arduino UNO, MEGA, NANO and similar, stm32 boards such as Nucleo and Bluepill, ESP32 and Teensy boards.
|
||||||
|
category=Device Control
|
||||||
|
url=https://docs.simplefoc.com
|
||||||
|
architectures=*
|
||||||
|
includes=SimpleFOC.h
|
||||||
42
lib/DRV8313/DRV8313.cpp
Normal file
42
lib/DRV8313/DRV8313.cpp
Normal file
|
|
@ -0,0 +1,42 @@
|
||||||
|
#include "DRV8313.h"
|
||||||
|
|
||||||
|
DRV8313Driver::DRV8313Driver(int phA, int phB, int phC, int en1, int en2, int en3, int slp, int rst,
|
||||||
|
int flt) : BLDCDriver3PWM(phA, phB, phC, en1, en2, en3), slp_pin(slp), rst_pin(rst),
|
||||||
|
flt_pin(flt) {
|
||||||
|
}
|
||||||
|
|
||||||
|
int DRV8313Driver::init() {
|
||||||
|
// Get state from flt pin
|
||||||
|
if (_isset(flt_pin)) {
|
||||||
|
pinMode(flt_pin, INPUT);
|
||||||
|
if (digitalRead(flt_pin) == HIGH) {
|
||||||
|
// if the fault pin is high the driver is in fault state
|
||||||
|
// reset the driver
|
||||||
|
if (_isset(rst_pin)) {
|
||||||
|
pinMode(rst_pin, OUTPUT);
|
||||||
|
digitalWrite(rst_pin, LOW);
|
||||||
|
delay(1);
|
||||||
|
digitalWrite(rst_pin, HIGH);
|
||||||
|
delay(1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return BLDCDriver3PWM::init();
|
||||||
|
}
|
||||||
|
|
||||||
|
void DRV8313Driver::enable() {
|
||||||
|
// Enable the driver
|
||||||
|
if (_isset(slp_pin)) {
|
||||||
|
pinMode(slp_pin, OUTPUT);
|
||||||
|
digitalWrite(slp_pin, HIGH);
|
||||||
|
}
|
||||||
|
BLDCDriver3PWM::enable();
|
||||||
|
}
|
||||||
|
|
||||||
|
void DRV8313Driver::disable() {
|
||||||
|
if (_isset(slp_pin)) {
|
||||||
|
pinMode(slp_pin, OUTPUT);
|
||||||
|
digitalWrite(slp_pin, LOW);
|
||||||
|
}
|
||||||
|
BLDCDriver3PWM::disable();
|
||||||
|
}
|
||||||
20
lib/DRV8313/DRV8313.h
Normal file
20
lib/DRV8313/DRV8313.h
Normal file
|
|
@ -0,0 +1,20 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "SimpleFOC.h"
|
||||||
|
|
||||||
|
class DRV8313Driver : public BLDCDriver3PWM {
|
||||||
|
public:
|
||||||
|
DRV8313Driver(int phA, int phB, int phC, int en1 = NOT_SET, int en2 = NOT_SET, int en3 = NOT_SET, int slp = NOT_SET,
|
||||||
|
int rst = NOT_SET, int flt = NOT_SET);
|
||||||
|
|
||||||
|
int init() override;
|
||||||
|
|
||||||
|
void enable() override;
|
||||||
|
|
||||||
|
void disable() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
int slp_pin;
|
||||||
|
int rst_pin;
|
||||||
|
int flt_pin;
|
||||||
|
};
|
||||||
10
lib/DRV8313/library.properties
Normal file
10
lib/DRV8313/library.properties
Normal file
|
|
@ -0,0 +1,10 @@
|
||||||
|
name=DRV8313 Simple FOC
|
||||||
|
version=1.0.0
|
||||||
|
author=vanyabeat <vanyabeat@protonmail.com>
|
||||||
|
maintainer=vanyabeat <vanyabeat@protonmail.com>
|
||||||
|
sentence=Simple library to work with DRV8313 and Simple FOC (for Robotroller in Robosemmbler)
|
||||||
|
paragraph=Simple library to work with DRV8313 and Simple FOC and simple library intended for hobby comunity to run the BLDC and Stepper motor using FOC algorithm. It is intended to support as many BLDC/Stepper motor+sensor+driver combinations as possible and in the same time maintain simplicity of usage. Library supports Arudino devices such as Arduino UNO, MEGA, NANO and similar, stm32 boards such as Nucleo and Bluepill, ESP32 and Teensy boards.
|
||||||
|
category=Device Control
|
||||||
|
url=https://docs.simplefoc.com
|
||||||
|
architectures=*
|
||||||
|
includes=SimpleFOC.h
|
||||||
46
lib/README
Normal file
46
lib/README
Normal file
|
|
@ -0,0 +1,46 @@
|
||||||
|
|
||||||
|
This directory is intended for project specific (private) libraries.
|
||||||
|
PlatformIO will compile them to static libraries and link into executable file.
|
||||||
|
|
||||||
|
The source code of each library should be placed in a an own separate directory
|
||||||
|
("lib/your_library_name/[here are source files]").
|
||||||
|
|
||||||
|
For example, see a structure of the following two libraries `Foo` and `Bar`:
|
||||||
|
|
||||||
|
|--lib
|
||||||
|
| |
|
||||||
|
| |--Bar
|
||||||
|
| | |--docs
|
||||||
|
| | |--examples
|
||||||
|
| | |--src
|
||||||
|
| | |- Bar.c
|
||||||
|
| | |- Bar.h
|
||||||
|
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
|
||||||
|
| |
|
||||||
|
| |--Foo
|
||||||
|
| | |- Foo.c
|
||||||
|
| | |- Foo.h
|
||||||
|
| |
|
||||||
|
| |- README --> THIS FILE
|
||||||
|
|
|
||||||
|
|- platformio.ini
|
||||||
|
|--src
|
||||||
|
|- main.c
|
||||||
|
|
||||||
|
and a contents of `src/main.c`:
|
||||||
|
```
|
||||||
|
#include <Foo.h>
|
||||||
|
#include <Bar.h>
|
||||||
|
|
||||||
|
int main (void)
|
||||||
|
{
|
||||||
|
...
|
||||||
|
}
|
||||||
|
|
||||||
|
```
|
||||||
|
|
||||||
|
PlatformIO Library Dependency Finder will find automatically dependent
|
||||||
|
libraries scanning project source files.
|
||||||
|
|
||||||
|
More information about PlatformIO Library Dependency Finder
|
||||||
|
- https://docs.platformio.org/page/librarymanager/ldf.html
|
||||||
116
mylink.ld
Normal file
116
mylink.ld
Normal file
|
|
@ -0,0 +1,116 @@
|
||||||
|
/*
|
||||||
|
* Линкер-скрипт для STM32F446RETx (512K FLASH, 128K RAM)
|
||||||
|
* Смещение FLASH: 0x08008000 (первые 32K под загрузчик)
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Точка входа */
|
||||||
|
ENTRY(Reset_Handler)
|
||||||
|
|
||||||
|
/* Конец стека (адрес начала стека = конец RAM) */
|
||||||
|
_estack = ORIGIN(RAM) + LENGTH(RAM);
|
||||||
|
|
||||||
|
/* Минимальные размеры кучи и стека */
|
||||||
|
_Min_Heap_Size = 0x1000; /* 4 КБ */
|
||||||
|
_Min_Stack_Size = 0x2000; /* 8 КБ */
|
||||||
|
|
||||||
|
/* Распределение памяти */
|
||||||
|
MEMORY {
|
||||||
|
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K
|
||||||
|
FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 384K # Для STM32F446 (512K - 32K)
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Секции */
|
||||||
|
SECTIONS {
|
||||||
|
/* Векторы прерываний (должны быть первыми!) */
|
||||||
|
.isr_vector : {
|
||||||
|
. = ALIGN(4);
|
||||||
|
KEEP(*(.isr_vector)) /* Секция векторов */
|
||||||
|
. = ALIGN(4);
|
||||||
|
} >FLASH
|
||||||
|
|
||||||
|
/* Программный код и константы */
|
||||||
|
.text : {
|
||||||
|
. = ALIGN(4);
|
||||||
|
*(.text) /* Код */
|
||||||
|
*(.text*) /* Код (включая C++) */
|
||||||
|
*(.glue_7) /* ARM/Thumb glue */
|
||||||
|
*(.glue_7t) /* Thumb/ARM glue */
|
||||||
|
*(.eh_frame)
|
||||||
|
|
||||||
|
KEEP(*(.init)) /* Конструкторы */
|
||||||
|
KEEP(*(.fini)) /* Деструкторы */
|
||||||
|
|
||||||
|
. = ALIGN(4);
|
||||||
|
_etext = .; /* Конец кода */
|
||||||
|
} >FLASH
|
||||||
|
|
||||||
|
/* Константы (только для чтения) */
|
||||||
|
.rodata : {
|
||||||
|
. = ALIGN(4);
|
||||||
|
*(.rodata) /* Константы */
|
||||||
|
*(.rodata*) /* Константы (включая строки) */
|
||||||
|
. = ALIGN(4);
|
||||||
|
} >FLASH
|
||||||
|
|
||||||
|
/* Таблицы для C++ (исключения, RTTI) */
|
||||||
|
.ARM.extab : {
|
||||||
|
. = ALIGN(4);
|
||||||
|
*(.ARM.extab* .gnu.linkonce.armextab.*)
|
||||||
|
. = ALIGN(4);
|
||||||
|
} >FLASH
|
||||||
|
|
||||||
|
.ARM.exidx : {
|
||||||
|
. = ALIGN(4);
|
||||||
|
__exidx_start = .;
|
||||||
|
*(.ARM.exidx*)
|
||||||
|
__exidx_end = .;
|
||||||
|
. = ALIGN(4);
|
||||||
|
} >FLASH
|
||||||
|
|
||||||
|
/* Инициализированные данные (копируются в RAM при старте) */
|
||||||
|
_sidata = LOADADDR(.data); /* Адрес данных во FLASH */
|
||||||
|
|
||||||
|
.data : {
|
||||||
|
. = ALIGN(4);
|
||||||
|
_sdata = .; /* Начало данных в RAM */
|
||||||
|
*(.data) /* Инициализированные переменные */
|
||||||
|
*(.data*) /* Инициализированные переменные (C++) */
|
||||||
|
*(.RamFunc) /* Функции в RAM */
|
||||||
|
*(.RamFunc*) /* Функции в RAM (C++) */
|
||||||
|
. = ALIGN(4);
|
||||||
|
_edata = .; /* Конец данных в RAM */
|
||||||
|
} >RAM AT> FLASH /* Физически хранятся во FLASH */
|
||||||
|
|
||||||
|
/* Неинициализированные данные (BSS) */
|
||||||
|
.bss : {
|
||||||
|
. = ALIGN(4);
|
||||||
|
_sbss = .; /* Начало BSS */
|
||||||
|
__bss_start__ = _sbss;
|
||||||
|
*(.bss) /* BSS переменные */
|
||||||
|
*(.bss*) /* BSS переменные (C++) */
|
||||||
|
*(COMMON) /* Общие переменные */
|
||||||
|
. = ALIGN(4);
|
||||||
|
_ebss = .; /* Конец BSS */
|
||||||
|
__bss_end__ = _ebss;
|
||||||
|
} >RAM
|
||||||
|
|
||||||
|
/* Куча и стек (резервируем место) */
|
||||||
|
._user_heap_stack : {
|
||||||
|
. = ALIGN(8);
|
||||||
|
PROVIDE(end = .);
|
||||||
|
PROVIDE(_end = .);
|
||||||
|
. = . + _Min_Heap_Size;
|
||||||
|
. = . + _Min_Stack_Size;
|
||||||
|
. = ALIGN(8);
|
||||||
|
} >RAM
|
||||||
|
|
||||||
|
/* Удаление ненужных секций из стандартных библиотек */
|
||||||
|
/DISCARD/ : {
|
||||||
|
libc.a (*)
|
||||||
|
libm.a (*)
|
||||||
|
libgcc.a (*)
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Атрибуты ARM */
|
||||||
|
.ARM.attributes 0 : { *(.ARM.attributes) }
|
||||||
|
}
|
||||||
33
platformio.ini
Normal file
33
platformio.ini
Normal file
|
|
@ -0,0 +1,33 @@
|
||||||
|
; PlatformIO Project Configuration File
|
||||||
|
;
|
||||||
|
; Build options: build flags, source filter
|
||||||
|
; Upload options: custom upload port, speed and extra flags
|
||||||
|
; Library options: dependencies, extra library storages
|
||||||
|
; Advanced options: extra scripting
|
||||||
|
;
|
||||||
|
; Please visit documentation for the other options and examples
|
||||||
|
; https://docs.platformio.org/page/projectconf.html
|
||||||
|
|
||||||
|
[platformio]
|
||||||
|
|
||||||
|
[env:robotroller_reborn]
|
||||||
|
platform = ststm32
|
||||||
|
board = genericSTM32F446RE
|
||||||
|
framework = arduino
|
||||||
|
#board_build.ldscript = mylink.ld
|
||||||
|
#board_upload.offset_address = 0x08008000
|
||||||
|
upload_protocol = stlink
|
||||||
|
debug_tool = stlink
|
||||||
|
monitor_speed = 19200
|
||||||
|
monitor_parity = N
|
||||||
|
build_flags =
|
||||||
|
-DSTM32F446xx
|
||||||
|
-D HAL_CAN_MODULE_ENABLED
|
||||||
|
-D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
|
||||||
|
lib_deps =
|
||||||
|
askuric/Simple FOC@^2.3.4
|
||||||
|
pazi88/STM32_CAN@^1.1.2
|
||||||
|
|
||||||
|
extra_scripts = pre:gen_compile_commands.py
|
||||||
|
|
||||||
|
|
||||||
199
src/flash.cpp
Normal file
199
src/flash.cpp
Normal file
|
|
@ -0,0 +1,199 @@
|
||||||
|
#include "flash.h"
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include "hal_conf_extra.h"
|
||||||
|
|
||||||
|
static uint32_t write_ptr = SECTOR_6;
|
||||||
|
|
||||||
|
void flash_unlock(){
|
||||||
|
|
||||||
|
// Check if flash is locked
|
||||||
|
if(!(FLASH->CR & FLASH_CR_LOCK)) {
|
||||||
|
return; // Already unlocked
|
||||||
|
}
|
||||||
|
|
||||||
|
// Write flash key sequence to unlock
|
||||||
|
FLASH->KEYR = 0x45670123; // First key
|
||||||
|
FLASH->KEYR = 0xCDEF89AB; // Second key
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void flash_lock() {
|
||||||
|
if(FLASH->CR & FLASH_CR_LOCK) {
|
||||||
|
return; // Already locked
|
||||||
|
}
|
||||||
|
FLASH->CR |= FLASH_CR_LOCK; // Lock flash memory
|
||||||
|
}
|
||||||
|
|
||||||
|
void erase_sector(uint8_t sector){
|
||||||
|
|
||||||
|
// Wait if flash is busy
|
||||||
|
while(FLASH_BUSY);
|
||||||
|
|
||||||
|
// Check if flash is locked and unlock if needed
|
||||||
|
if(FLASH->CR & FLASH_CR_LOCK) {
|
||||||
|
flash_unlock();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set sector erase bit and sector number
|
||||||
|
FLASH->CR |= FLASH_CR_SER;
|
||||||
|
FLASH->CR &= ~FLASH_CR_SNB;
|
||||||
|
FLASH->CR |= (sector << FLASH_CR_SNB_Pos) & FLASH_CR_SNB_Msk;
|
||||||
|
|
||||||
|
// Start erase
|
||||||
|
FLASH->CR |= FLASH_CR_STRT;
|
||||||
|
|
||||||
|
// Wait for erase to complete
|
||||||
|
while(FLASH_BUSY);
|
||||||
|
|
||||||
|
// Clear sector erase bit
|
||||||
|
FLASH->CR &= ~FLASH_CR_SER;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void flash_program_word(uint32_t address,uint32_t data,uint32_t byte_len){
|
||||||
|
|
||||||
|
// Wait if flash is busy
|
||||||
|
while(FLASH_BUSY);
|
||||||
|
// Check if flash is locked and unlock if needed
|
||||||
|
if(FLASH->CR & FLASH_CR_LOCK) {
|
||||||
|
flash_unlock();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set program bit 32bit programm size and Write data to address
|
||||||
|
if(byte_len == 1) {
|
||||||
|
FLASH_8BYTE;
|
||||||
|
FLASH->CR |= FLASH_CR_PG;
|
||||||
|
*(volatile uint8_t*)address = (uint8_t)data;
|
||||||
|
} else {
|
||||||
|
FLASH_32BYTE;
|
||||||
|
FLASH->CR |= FLASH_CR_PG;
|
||||||
|
*(volatile uint32_t*)address = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Wait for programming to complete
|
||||||
|
while(FLASH_BUSY);
|
||||||
|
|
||||||
|
// Clear program bit
|
||||||
|
FLASH->CR &= ~FLASH_CR_PG;
|
||||||
|
|
||||||
|
}
|
||||||
|
void flash_write(uint32_t addr, FLASH_RECORD* record){
|
||||||
|
|
||||||
|
uint32_t* data = (uint32_t*)record;
|
||||||
|
uint32_t size = FLASH_RECORD_SIZE / 4; //count words in struct
|
||||||
|
// Wait if flash is busy
|
||||||
|
while(FLASH_BUSY);
|
||||||
|
|
||||||
|
// Check if flash is locked and unlock if needed
|
||||||
|
if(FLASH->CR & FLASH_CR_LOCK) {
|
||||||
|
flash_unlock();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set program bit and write data to flash
|
||||||
|
FLASH_32BYTE;
|
||||||
|
FLASH->CR |= FLASH_CR_PG;
|
||||||
|
|
||||||
|
for(int i = 0;i < size;i++){
|
||||||
|
*(volatile uint32_t*)(addr + i) = data[i];
|
||||||
|
write_ptr++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clear program bit
|
||||||
|
FLASH->CR &= ~FLASH_CR_PG;
|
||||||
|
flash_lock();
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t flash_read_word(uint32_t address){
|
||||||
|
|
||||||
|
// Check if address is valid
|
||||||
|
if(address < FLASH_BASE || address > FLASH_END) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Read byte from flash memory
|
||||||
|
return *((volatile uint8_t*)address);
|
||||||
|
|
||||||
|
}
|
||||||
|
// Wait if flash
|
||||||
|
|
||||||
|
|
||||||
|
bool validate_crc(FLASH_RECORD* crc){
|
||||||
|
if(crc->crc == 6933)
|
||||||
|
return true;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
/* read struct from FLASH */
|
||||||
|
void flash_read(uint32_t addr,FLASH_RECORD* ptr){
|
||||||
|
uint8_t* flash_ptr = (uint8_t*)addr;
|
||||||
|
uint8_t* dest = (uint8_t*)ptr;
|
||||||
|
for(int i = 0;i < FLASH_RECORD_SIZE;i++)
|
||||||
|
dest[i] = flash_ptr[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
void compact_page(){
|
||||||
|
FLASH_RECORD latest[PARAM_COUNT] = {0};
|
||||||
|
for(int i = (uint32_t)SECTOR_6;i < (uint32_t)SECTOR_7;i += FLASH_RECORD_SIZE) {
|
||||||
|
FLASH_RECORD rec;
|
||||||
|
flash_read(i,&rec);
|
||||||
|
|
||||||
|
if (validate_crc(&rec)){
|
||||||
|
latest[rec.data_id] = rec;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
erase_sector(6);
|
||||||
|
write_ptr = SECTOR_6; // Сброс на начало
|
||||||
|
|
||||||
|
for (int i = 0; i < PARAM_COUNT; i++) {
|
||||||
|
if (latest[i].data_id != 0xFF) {
|
||||||
|
// Выравнивание перед каждой записью
|
||||||
|
if (write_ptr % 4 != 0) {
|
||||||
|
write_ptr += (4 - (write_ptr % 4));
|
||||||
|
}
|
||||||
|
flash_write(write_ptr, &latest[i]);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void write_param(uint8_t param_id, uint8_t val) {
|
||||||
|
FLASH_RECORD param_flash = {param_id, val, 6933};
|
||||||
|
|
||||||
|
__disable_irq(); // Запрещаем прерывания на время всей операции
|
||||||
|
|
||||||
|
// Проверка выравнивания ДО проверки границ сектора
|
||||||
|
if (write_ptr % 4 != 0) {
|
||||||
|
write_ptr += (4 - (write_ptr % 4));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Проверка переполнения с учётом выравнивания
|
||||||
|
if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) {
|
||||||
|
compact_page(); // После compact_page write_ptr обновляется
|
||||||
|
// Повторно выравниваем после компактификации. То есть сколько не хватает для кратности
|
||||||
|
if (write_ptr % 4 != 0) {
|
||||||
|
write_ptr += (4 - (write_ptr % 4));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
flash_write(write_ptr, ¶m_flash);
|
||||||
|
write_ptr += FLASH_RECORD_SIZE;
|
||||||
|
|
||||||
|
__enable_irq(); // Разрешаем прерывания
|
||||||
|
}
|
||||||
|
|
||||||
|
FLASH_RECORD* load_params(){
|
||||||
|
__disable_irq();
|
||||||
|
static FLASH_RECORD latest[PARAM_COUNT] = {0};
|
||||||
|
FLASH_RECORD res;
|
||||||
|
for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) {
|
||||||
|
flash_read(addr,&res);
|
||||||
|
if (!validate_crc(&res))
|
||||||
|
break;
|
||||||
|
else{
|
||||||
|
latest[res.data_id] = res;
|
||||||
|
}
|
||||||
|
write_ptr = addr + FLASH_RECORD_SIZE;
|
||||||
|
}
|
||||||
|
|
||||||
|
__enable_irq();
|
||||||
|
return latest;
|
||||||
|
}
|
||||||
323
src/main.cpp
Normal file
323
src/main.cpp
Normal file
|
|
@ -0,0 +1,323 @@
|
||||||
|
// clang-format off
|
||||||
|
#include "Arduino.h"
|
||||||
|
#include "stm32f446xx.h"
|
||||||
|
#include <SimpleFOC.h>
|
||||||
|
#include <STM32_CAN.h>
|
||||||
|
#include <AS5045.h>
|
||||||
|
#include <DRV8313.h>
|
||||||
|
#include <cstring>
|
||||||
|
#include <iterator>
|
||||||
|
#include "common/base_classes/FOCMotor.h"
|
||||||
|
#include "hal_conf_extra.h"
|
||||||
|
#include "wiring_analog.h"
|
||||||
|
#include "wiring_constants.h"
|
||||||
|
// clang-format on
|
||||||
|
#include "reg_cah.h"
|
||||||
|
#include "flash.h"
|
||||||
|
|
||||||
|
|
||||||
|
void SysTick_Handler(void) {
|
||||||
|
HAL_IncTick();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
STM32_CAN Can(CAN2, DEF);
|
||||||
|
/* for FLASH */
|
||||||
|
uint32_t flash_flag;
|
||||||
|
uint8_t flag_can = 0;
|
||||||
|
uint32_t flash_error;
|
||||||
|
FLASH_EraseInitTypeDef pEraseInit;
|
||||||
|
uint32_t SectorError;
|
||||||
|
|
||||||
|
volatile uint32_t msg_id;
|
||||||
|
volatile uint16_t id_x;
|
||||||
|
volatile uint8_t msg_ch;
|
||||||
|
|
||||||
|
volatile float kt = 0.1; //for torgue calculation
|
||||||
|
|
||||||
|
static FLASH_RECORD* flash_rec;
|
||||||
|
static FLASH_RECORD flash_buf[PARAM_COUNT];
|
||||||
|
static CAN_message_t CAN_TX_msg;
|
||||||
|
static CAN_message_t CAN_inMsg;
|
||||||
|
|
||||||
|
SPIClass spi;
|
||||||
|
MagneticSensorAS5045 encoder(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);
|
||||||
|
|
||||||
|
BLDCMotor motor(POLE_PAIRS);
|
||||||
|
|
||||||
|
DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER,
|
||||||
|
EN_U_GATE_DRIVER, EN_V_GATE_DRIVER, SLEEP_DRIVER,
|
||||||
|
RESET_DRIVER, FAULT_DRIVER);
|
||||||
|
LowsideCurrentSense current_sense(0.01, 10.0, CURRENT_SENSOR_1,
|
||||||
|
CURRENT_SENSOR_2, CURRENT_SENSOR_3);
|
||||||
|
|
||||||
|
Commander command(Serial);
|
||||||
|
|
||||||
|
struct MotorControlInputs {
|
||||||
|
float target_angle = 0.0;
|
||||||
|
float target_velocity = 0.0;
|
||||||
|
bool motor_enabled = false;
|
||||||
|
bool foc_state = false;
|
||||||
|
};
|
||||||
|
|
||||||
|
MotorControlInputs motor_control_inputs;
|
||||||
|
|
||||||
|
void doMotor(char *cmd) {
|
||||||
|
command.motor(&motor, cmd);
|
||||||
|
digitalWrite(PC10, !digitalRead(PC10));
|
||||||
|
delayMicroseconds(2);
|
||||||
|
}
|
||||||
|
|
||||||
|
void CAN2_RX0_IRQHandler() {
|
||||||
|
// Пустая функция, но прерывание не приведет к Default Handler
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
|
||||||
|
DRV8313Driver *driver, LowsideCurrentSense *current_sense,
|
||||||
|
Commander *commander, CommandCallback callback) {
|
||||||
|
encoder->init(&spi);
|
||||||
|
|
||||||
|
driver->pwm_frequency = 20000;
|
||||||
|
driver->voltage_power_supply = 24;
|
||||||
|
driver->voltage_limit = 24;
|
||||||
|
driver->init();
|
||||||
|
|
||||||
|
current_sense->linkDriver(driver);
|
||||||
|
current_sense->init();
|
||||||
|
|
||||||
|
motor->linkSensor(encoder);
|
||||||
|
motor->linkDriver(driver);
|
||||||
|
motor->linkCurrentSense(current_sense);
|
||||||
|
motor->useMonitoring(Serial);
|
||||||
|
motor->monitor_downsample = 5000; // default 0
|
||||||
|
motor->controller = MotionControlType::angle;
|
||||||
|
motor->torque_controller = TorqueControlType::voltage;
|
||||||
|
motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||||
|
|
||||||
|
// PID start
|
||||||
|
motor->PID_velocity.P = 0.75;
|
||||||
|
motor->PID_velocity.I = 20;
|
||||||
|
motor->LPF_velocity.Tf = 0.005;
|
||||||
|
motor->P_angle.P = 0.5;
|
||||||
|
motor->LPF_angle.Tf = 0.001;
|
||||||
|
// PID end
|
||||||
|
|
||||||
|
motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm)
|
||||||
|
motor->voltage_limit = 24;
|
||||||
|
motor->current_limit = 0.5;
|
||||||
|
|
||||||
|
motor->sensor_direction = Direction::CCW;
|
||||||
|
motor->init();
|
||||||
|
motor->initFOC();
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_velocity() {
|
||||||
|
float current_velocity = motor.shaftVelocity();
|
||||||
|
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
|
||||||
|
CAN_TX_msg.id = id;
|
||||||
|
CAN_TX_msg.buf[0] = 'V';
|
||||||
|
CAN_TX_msg.len = 5;
|
||||||
|
memcpy(&CAN_TX_msg.buf[1], ¤t_velocity, sizeof(current_velocity));
|
||||||
|
Can.write(CAN_TX_msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_angle() {
|
||||||
|
float current_angle = motor.shaftAngle();
|
||||||
|
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
|
||||||
|
CAN_TX_msg.id = id;
|
||||||
|
CAN_TX_msg.buf[0] = 'A';
|
||||||
|
CAN_TX_msg.len = 5;
|
||||||
|
memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
||||||
|
Can.write(CAN_TX_msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_motor_enabled() {
|
||||||
|
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
|
||||||
|
CAN_TX_msg.id = id;
|
||||||
|
CAN_TX_msg.buf[0] = 'E';
|
||||||
|
memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled,
|
||||||
|
sizeof(motor_control_inputs.motor_enabled));
|
||||||
|
Can.write(CAN_TX_msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_foc_state() {
|
||||||
|
uint8_t id = *(volatile uint8_t*)ADDR_VAR;
|
||||||
|
CAN_TX_msg.id = id;
|
||||||
|
CAN_TX_msg.buf[0] = 'F';
|
||||||
|
memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.foc_state,
|
||||||
|
sizeof(motor_control_inputs.foc_state));
|
||||||
|
Can.write(CAN_TX_msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_id() {
|
||||||
|
/* data for reading of firmware */
|
||||||
|
flash_rec = load_params();
|
||||||
|
CAN_TX_msg.id = flash_rec->value;
|
||||||
|
CAN_TX_msg.buf[0] = 'I';
|
||||||
|
memcpy(&CAN_TX_msg.buf[1], &(flash_rec->value), sizeof(uint8_t));
|
||||||
|
Can.write(CAN_TX_msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_motor_torque() {
|
||||||
|
float i_q = motor.current.q; // Ток по оси q (А)
|
||||||
|
float torque = kt * i_q; // Расчет момента
|
||||||
|
torque *= 100;
|
||||||
|
flash_rec = load_params();
|
||||||
|
CAN_TX_msg.id = flash_rec->value;
|
||||||
|
CAN_TX_msg.buf[0] = 'T';
|
||||||
|
CAN_TX_msg.len = 5;
|
||||||
|
memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
|
||||||
|
Can.write(CAN_TX_msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup_id(uint8_t my_id) {
|
||||||
|
write_param(addr_id,my_id);
|
||||||
|
send_id();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void send_data() {
|
||||||
|
// send_velocity();
|
||||||
|
// send_angle();
|
||||||
|
// send_motor_enabled();
|
||||||
|
// read_temperature();
|
||||||
|
// GPIOC->ODR ^= GPIO_ODR_OD11;
|
||||||
|
}
|
||||||
|
|
||||||
|
void listen_can(const CAN_message_t &msg) {
|
||||||
|
msg_id = msg.id;
|
||||||
|
/* 0x691
|
||||||
|
69 - адрес устройства
|
||||||
|
1 - что делать дальше с данными */
|
||||||
|
msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать
|
||||||
|
id_x = (msg_id >> 4) & 0x7FF; //получение адреса устройства страшие 2 бита msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать
|
||||||
|
if(id_x == flash_rec->value){
|
||||||
|
if(msg_ch == REG_WRITE){
|
||||||
|
switch(msg.buf[0]) {
|
||||||
|
case REG_ID:
|
||||||
|
/* setup new id */
|
||||||
|
setup_id(msg.buf[1]);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case REG_LED_BLINK:
|
||||||
|
for (int i = 0; i < 10; i++) {
|
||||||
|
GPIOC->ODR ^= GPIO_ODR_OD10;
|
||||||
|
delay(100);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_ENABLED:
|
||||||
|
if (msg.buf[1] == 1) {
|
||||||
|
motor.enable();
|
||||||
|
motor_control_inputs.motor_enabled = 1;
|
||||||
|
} else {
|
||||||
|
motor.disable();
|
||||||
|
motor_control_inputs.motor_enabled = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (msg_ch == REG_READ) {
|
||||||
|
switch (msg.buf[0]) {
|
||||||
|
case REG_ID:
|
||||||
|
send_id();
|
||||||
|
break;
|
||||||
|
case MOTOR_VELOCITY:
|
||||||
|
send_velocity();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_ANGLE:
|
||||||
|
send_angle();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_ENABLED:
|
||||||
|
send_motor_enabled();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_TORQUE:
|
||||||
|
send_motor_torque();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case FOC_STATE:
|
||||||
|
send_foc_state();
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
volatile uint32_t ipsr_value = 0;
|
||||||
|
|
||||||
|
|
||||||
|
void foc_step(BLDCMotor *motor, Commander *commander) {
|
||||||
|
if (motor_control_inputs.target_velocity != 0 ||
|
||||||
|
motor->controller == MotionControlType::velocity) {
|
||||||
|
if (motor->controller != MotionControlType::velocity) {
|
||||||
|
motor->controller = MotionControlType::velocity;
|
||||||
|
}
|
||||||
|
motor->target = motor_control_inputs.target_velocity;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
if (motor->controller != MotionControlType::angle) {
|
||||||
|
motor->controller = MotionControlType::angle;
|
||||||
|
}
|
||||||
|
motor->target = motor_control_inputs.target_angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
motor->loopFOC();
|
||||||
|
motor->move();
|
||||||
|
motor->monitor();
|
||||||
|
commander->run();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void setup(){
|
||||||
|
/* bias for vector int */
|
||||||
|
// __set_MSP(*(volatile uint32_t*)0x08008000);
|
||||||
|
// SCB->VTOR = (volatile uint32_t)0x08008000;
|
||||||
|
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
|
||||||
|
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
|
||||||
|
Serial.begin(115200);
|
||||||
|
|
||||||
|
pinMode(PC11, OUTPUT);
|
||||||
|
pinMode(PC10,OUTPUT);
|
||||||
|
GPIOC->ODR &= ~GPIO_ODR_OD10;
|
||||||
|
// Setup thermal sensor pin
|
||||||
|
// pinMode(TH1, INPUT_ANALOG);
|
||||||
|
Can.begin();
|
||||||
|
Can.setBaudRate(1000000);
|
||||||
|
TIM_TypeDef *Instance = TIM2;
|
||||||
|
HardwareTimer *SendTimer = new HardwareTimer(Instance);
|
||||||
|
// SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz
|
||||||
|
// SendTimer->attachInterrupt(send_data);
|
||||||
|
// SendTimer->resume();
|
||||||
|
flash_rec = load_params();
|
||||||
|
for(int i = 0;i < PARAM_COUNT;i++)
|
||||||
|
flash_buf[i] = flash_rec[i];
|
||||||
|
setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor);
|
||||||
|
GPIOC->ODR |= GPIO_ODR_OD11;
|
||||||
|
motor.torque_controller = TorqueControlType::foc_current;
|
||||||
|
motor.controller = MotionControlType::torque;
|
||||||
|
__enable_irq();
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
foc_step(&motor, &command);
|
||||||
|
CAN_message_t msg;
|
||||||
|
GPIOC->ODR ^= GPIO_ODR_OD11;
|
||||||
|
delay(500);
|
||||||
|
while (Can.read(msg)) {
|
||||||
|
listen_can(msg);
|
||||||
|
}
|
||||||
|
}
|
||||||
86
test/README.md
Normal file
86
test/README.md
Normal file
|
|
@ -0,0 +1,86 @@
|
||||||
|
# CAN Communication Scripts
|
||||||
|
|
||||||
|
This repository contains Python scripts for testing and interacting with a CAN bus system. These scripts enable sending and receiving CAN messages to control a motor, set angles, and adjust velocities.
|
||||||
|
|
||||||
|
## Prerequisites
|
||||||
|
|
||||||
|
1. **Python 3.7+** installed on your system.
|
||||||
|
2. **`python-can` library** installed. Install it via pip:
|
||||||
|
```bash
|
||||||
|
pip install python-can
|
||||||
|
```
|
||||||
|
3. **SocketCAN interface** properly configured on your Linux system. The default channel is `can0`.
|
||||||
|
|
||||||
|
## Usage
|
||||||
|
|
||||||
|
### 1. Receiving CAN Messages
|
||||||
|
|
||||||
|
The script `python_can.py` listens to the CAN bus and processes incoming messages.
|
||||||
|
|
||||||
|
#### Run:
|
||||||
|
```bash
|
||||||
|
python3 python_can.py
|
||||||
|
```
|
||||||
|
|
||||||
|
#### Features:
|
||||||
|
- Processes messages with data length 5.
|
||||||
|
- Parses the first byte (`flag`) to determine the type:
|
||||||
|
- `'A'`: Angle (float).
|
||||||
|
- `'V'`: Velocity (float).
|
||||||
|
- `'E'`: Enable/disable status (boolean).
|
||||||
|
|
||||||
|
### 2. Enabling or Disabling the Motor
|
||||||
|
|
||||||
|
The script `python_enable_motor.py` sends commands to enable or disable the motor.
|
||||||
|
|
||||||
|
#### Run:
|
||||||
|
```bash
|
||||||
|
python3 python_enable_motor.py <0|1>
|
||||||
|
```
|
||||||
|
|
||||||
|
#### Arguments:
|
||||||
|
- `0`: Disable the motor.
|
||||||
|
- `1`: Enable the motor.
|
||||||
|
|
||||||
|
### 3. Sending Target Angle
|
||||||
|
|
||||||
|
The script `python_send_angle.py` sends a target angle to the CAN bus.
|
||||||
|
|
||||||
|
#### Run:
|
||||||
|
```bash
|
||||||
|
python3 python_send_angle.py
|
||||||
|
```
|
||||||
|
|
||||||
|
#### Behavior:
|
||||||
|
- Sends a message with a predefined target angle every second.
|
||||||
|
- Adjust the target angle in the script (`target_angle` variable).
|
||||||
|
|
||||||
|
### 4. Sending Target Velocity
|
||||||
|
|
||||||
|
The script `python_send_velocity.py` sends a target velocity to the CAN bus.
|
||||||
|
|
||||||
|
#### Run:
|
||||||
|
```bash
|
||||||
|
python3 python_send_velocity.py
|
||||||
|
```
|
||||||
|
|
||||||
|
#### Behavior:
|
||||||
|
- Sends a message with a predefined target velocity every second.
|
||||||
|
- Adjust the target velocity in the script (`target_speed` variable).
|
||||||
|
|
||||||
|
## Configuration
|
||||||
|
|
||||||
|
### CAN Interface
|
||||||
|
The scripts use the following default CAN bus settings:
|
||||||
|
- **Channel**: `can0`
|
||||||
|
- **Bitrate**: `1 Mbps`
|
||||||
|
|
||||||
|
If your configuration differs, update the `Bus()` initialization in the scripts.
|
||||||
|
|
||||||
|
## Troubleshooting
|
||||||
|
|
||||||
|
1. **"Error initializing CAN bus"**:
|
||||||
|
- Ensure your CAN interface is correctly configured and active:
|
||||||
|
```bash
|
||||||
|
sudo ip link set can0 up type can bitrate 1000000
|
||||||
|
```
|
||||||
47
test/python_can.py
Normal file
47
test/python_can.py
Normal file
|
|
@ -0,0 +1,47 @@
|
||||||
|
import can
|
||||||
|
import struct
|
||||||
|
import time
|
||||||
|
|
||||||
|
def process_can_message(msg):
|
||||||
|
if msg.dlc == 5: # Check the message length
|
||||||
|
print(f"Received message with ID: {msg.arbitration_id}")
|
||||||
|
print(f"Data: {msg.data}")
|
||||||
|
|
||||||
|
# The first byte determines the data type (flag)
|
||||||
|
flag = chr(msg.data[0])
|
||||||
|
|
||||||
|
if flag == 'A': # Angle
|
||||||
|
angle_bytes = msg.data[1:5]
|
||||||
|
angle = struct.unpack('<f', bytes(angle_bytes))[0]
|
||||||
|
print(f"Angle: {angle} degrees")
|
||||||
|
elif flag == 'V': # Velocity
|
||||||
|
velocity_bytes = msg.data[1:5]
|
||||||
|
velocity = struct.unpack('<f', bytes(velocity_bytes))[0]
|
||||||
|
print(f"Velocity: {velocity} rad/s")
|
||||||
|
elif flag == 'E' and msg.dlc >= 2: # Enable/Disable
|
||||||
|
enabled = msg.data[1] # Expecting 1 byte (0 or 1)
|
||||||
|
print(f"Enabled: {bool(enabled)}")
|
||||||
|
else:
|
||||||
|
print(f"Unknown flag: {flag}")
|
||||||
|
else:
|
||||||
|
print(f"Received message with unexpected length: {msg.dlc}")
|
||||||
|
|
||||||
|
def receive_can_messages():
|
||||||
|
try:
|
||||||
|
# Connect to the CAN bus
|
||||||
|
bus = can.interface.Bus(channel='can0', bustype='socketcan')
|
||||||
|
|
||||||
|
print("Waiting for messages on the CAN bus...")
|
||||||
|
|
||||||
|
while True:
|
||||||
|
msg = bus.recv()
|
||||||
|
if msg:
|
||||||
|
process_can_message(msg)
|
||||||
|
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("\nExiting program...")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"Error: {e}")
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
receive_can_messages()
|
||||||
54
test/python_enable_motor.py
Normal file
54
test/python_enable_motor.py
Normal file
|
|
@ -0,0 +1,54 @@
|
||||||
|
import can
|
||||||
|
import sys
|
||||||
|
|
||||||
|
# Function to send the motor enable/disable command
|
||||||
|
def send_motor_enable(bus, enable):
|
||||||
|
"""
|
||||||
|
Sends a command to enable or disable the motor.
|
||||||
|
|
||||||
|
:param bus: The CAN bus
|
||||||
|
:param enable: 1 to enable the motor, 0 to disable it
|
||||||
|
"""
|
||||||
|
msg = can.Message()
|
||||||
|
msg.arbitration_id = 1 # Message ID
|
||||||
|
msg.is_extended_id = False
|
||||||
|
msg.dlc = 2 # Message length (flag + 1 byte of data)
|
||||||
|
msg.data = [ord('E'), enable] # 'E' for the command, followed by 0 or 1
|
||||||
|
|
||||||
|
try:
|
||||||
|
bus.send(msg)
|
||||||
|
state = "enabled" if enable else "disabled"
|
||||||
|
print(f"Sent message to {state} motor")
|
||||||
|
print(f"Message data: {msg.data}")
|
||||||
|
except can.CanError as e:
|
||||||
|
print(f"Message failed to send: {e}")
|
||||||
|
sys.exit(1) # Exit the program on failure
|
||||||
|
|
||||||
|
def main():
|
||||||
|
# CAN interface setup
|
||||||
|
bus = None # Define outside the try block for proper shutdown
|
||||||
|
try:
|
||||||
|
bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings
|
||||||
|
print("CAN bus initialized.")
|
||||||
|
|
||||||
|
# Ensure the state is passed via arguments
|
||||||
|
if len(sys.argv) != 2 or sys.argv[1] not in ['0', '1']:
|
||||||
|
print("Usage: python3 script_name.py <0|1>")
|
||||||
|
print("0 - Disable motor, 1 - Enable motor")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
enable = int(sys.argv[1])
|
||||||
|
send_motor_enable(bus, enable)
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
print(f"Error initializing CAN bus: {e}")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
finally:
|
||||||
|
# Ensure the bus is properly shut down
|
||||||
|
if bus is not None:
|
||||||
|
bus.shutdown()
|
||||||
|
print("CAN bus shut down.")
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
37
test/python_send_angle.py
Normal file
37
test/python_send_angle.py
Normal file
|
|
@ -0,0 +1,37 @@
|
||||||
|
import can
|
||||||
|
import struct
|
||||||
|
import time
|
||||||
|
import argparse
|
||||||
|
|
||||||
|
# Function to send the target angle
|
||||||
|
def send_target_angle(bus, target_angle):
|
||||||
|
msg = can.Message()
|
||||||
|
msg.arbitration_id = 1 # Message ID
|
||||||
|
msg.is_extended_id = False
|
||||||
|
msg.dlc = 5 # Message length
|
||||||
|
msg.data = [ord('A')] + list(struct.pack('<f', target_angle)) # 'A' for the command identifier, followed by the angle in float format
|
||||||
|
|
||||||
|
try:
|
||||||
|
bus.send(msg)
|
||||||
|
print(f"Sent message with target angle: {target_angle} degrees")
|
||||||
|
print(f"Message data: {msg.data}")
|
||||||
|
except can.CanError:
|
||||||
|
print("Message failed to send")
|
||||||
|
|
||||||
|
# Main function
|
||||||
|
def main():
|
||||||
|
parser = argparse.ArgumentParser(description="Send target angles over CAN bus.")
|
||||||
|
parser.add_argument("--angle", type=float, required=True, help="Target angle to send over the CAN bus")
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
target_angle = args.angle
|
||||||
|
|
||||||
|
# CAN interface setup
|
||||||
|
bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings
|
||||||
|
print("CAN bus initialized, sending target angles...")
|
||||||
|
|
||||||
|
# Loop to send messages
|
||||||
|
send_target_angle(bus, target_angle)
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
76
test/python_send_velocity.py
Normal file
76
test/python_send_velocity.py
Normal file
|
|
@ -0,0 +1,76 @@
|
||||||
|
import can
|
||||||
|
import struct
|
||||||
|
import time
|
||||||
|
import sys
|
||||||
|
|
||||||
|
# Function to send the target speed
|
||||||
|
def send_target_speed(bus, target_speed):
|
||||||
|
msg = can.Message()
|
||||||
|
msg.arbitration_id = 1 # Message ID
|
||||||
|
msg.is_extended_id = False
|
||||||
|
msg.dlc = 5 # Message length
|
||||||
|
msg.data = bytearray([ord('V')] + list(struct.pack('<f', target_speed))) # 'V' for the command identifier, followed by the speed in float format
|
||||||
|
|
||||||
|
try:
|
||||||
|
bus.send(msg)
|
||||||
|
print(f"Sent message with target speed: {target_speed} rad/s")
|
||||||
|
except can.CanError:
|
||||||
|
print("Message failed to send")
|
||||||
|
|
||||||
|
# Function to send the motor enable/disable command
|
||||||
|
def send_motor_enable(bus, enable):
|
||||||
|
"""
|
||||||
|
Sends a command to enable or disable the motor.
|
||||||
|
|
||||||
|
:param bus: The CAN bus
|
||||||
|
:param enable: 1 to enable the motor, 0 to disable it
|
||||||
|
"""
|
||||||
|
msg = can.Message()
|
||||||
|
msg.arbitration_id = 1 # Message ID
|
||||||
|
msg.is_extended_id = False
|
||||||
|
msg.dlc = 2 # Message length (flag + 1 byte of data)
|
||||||
|
msg.data = bytearray([ord('E'), enable]) # 'E' for the command, followed by 0 or 1
|
||||||
|
|
||||||
|
try:
|
||||||
|
bus.send(msg)
|
||||||
|
state = "enabled" if enable else "disabled"
|
||||||
|
print(f"Sent message to {state} motor")
|
||||||
|
except can.CanError as e:
|
||||||
|
print(f"Message failed to send: {e}")
|
||||||
|
sys.exit(1) # Exit the program on failure
|
||||||
|
|
||||||
|
send_target_speed(bus,0.0)
|
||||||
|
|
||||||
|
def main():
|
||||||
|
# CAN interface setup
|
||||||
|
bus = None # Define outside the try block for proper shutdown
|
||||||
|
try:
|
||||||
|
bus = can.interface.Bus(channel='COM4', bustype='slcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings
|
||||||
|
print("CAN bus initialized.")
|
||||||
|
|
||||||
|
while True:
|
||||||
|
user_input = input("Enter target speed: ")
|
||||||
|
if user_input.lower() == 'exit':
|
||||||
|
print("Exiting...")
|
||||||
|
break
|
||||||
|
try:
|
||||||
|
target_speed = float(user_input)
|
||||||
|
send_target_speed(bus, target_speed)
|
||||||
|
except ValueError:
|
||||||
|
print("Invalid input. Please enter a valid number.")
|
||||||
|
|
||||||
|
# Disable motor before exiting
|
||||||
|
send_motor_enable(bus, 0)
|
||||||
|
print("Motor disabled.")
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
print(f"Error initializing1 CAN bus: {e}")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
finally:
|
||||||
|
if bus is not None:
|
||||||
|
bus.shutdown()
|
||||||
|
print("CAN bus shut down.")
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
142
test/python_test_boot.py
Normal file
142
test/python_test_boot.py
Normal file
|
|
@ -0,0 +1,142 @@
|
||||||
|
import can
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
from intelhex import IntelHex
|
||||||
|
from crc import Calculator, Crc16
|
||||||
|
# Конфигурация
|
||||||
|
CAN_CHANNEL = 'socketcan'
|
||||||
|
CAN_INTERFACE = 'can0'
|
||||||
|
CAN_BITRATE = 1000000
|
||||||
|
#ch =int(input("Введите id устройства:"))
|
||||||
|
ch = int(sys.argv[2])
|
||||||
|
BOOT_CAN_ID = (ch * 16) + 1
|
||||||
|
DATA_CAN_ID = (ch * 16) + 3
|
||||||
|
BOOT_CAN_END = (ch * 16) + 2
|
||||||
|
ACK_CAN_ID = 0x05
|
||||||
|
|
||||||
|
#конфиг для crc16 ibm
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def debug_print(msg):
|
||||||
|
print(f"[DEBUG] {msg}")
|
||||||
|
|
||||||
|
def calculate_crc16_modbus(data: bytes) -> int:
|
||||||
|
crc = 0xFFFF
|
||||||
|
for byte in data:
|
||||||
|
crc ^= byte
|
||||||
|
for _ in range(8):
|
||||||
|
if crc & 0x0001:
|
||||||
|
crc = (crc >> 1) ^ 0xA001
|
||||||
|
else:
|
||||||
|
crc >>= 1
|
||||||
|
return crc
|
||||||
|
|
||||||
|
def send_firmware(hex_file):
|
||||||
|
try:
|
||||||
|
debug_print("Инициализация CAN...")
|
||||||
|
bus = can.interface.Bus(
|
||||||
|
channel=CAN_INTERFACE,
|
||||||
|
bustype=CAN_CHANNEL,
|
||||||
|
bitrate=CAN_BITRATE
|
||||||
|
)
|
||||||
|
|
||||||
|
debug_print("Чтение HEX-файла...")
|
||||||
|
ih = IntelHex(hex_file)
|
||||||
|
binary_data = ih.tobinstr() # Исправлено на tobinstr()
|
||||||
|
fw_size = len(binary_data)
|
||||||
|
debug_print(f"Размер прошивки: {fw_size} байт")
|
||||||
|
|
||||||
|
# Расчет CRC
|
||||||
|
debug_print("Расчёт CRC...")
|
||||||
|
calculator = Calculator(Crc16.IBM)
|
||||||
|
fw_crc = calculate_crc16_modbus(binary_data)
|
||||||
|
debug_print(f"CRC: 0x{fw_crc:04X}")
|
||||||
|
|
||||||
|
# Отправка START
|
||||||
|
start_data = bytearray([0x01])
|
||||||
|
start_data += fw_size.to_bytes(4, 'little')
|
||||||
|
start_data += fw_crc.to_bytes(2, 'little')
|
||||||
|
|
||||||
|
debug_print(f"START: {list(start_data)}")
|
||||||
|
start_msg = can.Message(
|
||||||
|
arbitration_id=BOOT_CAN_ID,
|
||||||
|
data=bytes(start_data),
|
||||||
|
is_extended_id=False
|
||||||
|
)
|
||||||
|
|
||||||
|
try:
|
||||||
|
bus.send(start_msg)
|
||||||
|
except can.CanError as e:
|
||||||
|
debug_print(f"Ошибка отправки START: {str(e)}")
|
||||||
|
return
|
||||||
|
|
||||||
|
# Ожидание ACK
|
||||||
|
debug_print("Ожидание ACK...")
|
||||||
|
ack = wait_for_ack(bus)
|
||||||
|
if not ack:
|
||||||
|
debug_print("Таймаут ACK START")
|
||||||
|
return
|
||||||
|
debug_print(f"Получен ACK: {list(ack.data)}")
|
||||||
|
|
||||||
|
# Отправка данных
|
||||||
|
packet_size = 8
|
||||||
|
for i in range(0, len(binary_data), packet_size):
|
||||||
|
chunk = binary_data[i:i+packet_size]
|
||||||
|
# Дополнение до 8 байт
|
||||||
|
if len(chunk) < 8:
|
||||||
|
chunk += b'\xFF' * (8 - len(chunk))
|
||||||
|
|
||||||
|
debug_print(f"Пакет {i//8}: {list(chunk)}")
|
||||||
|
data_msg = can.Message(
|
||||||
|
arbitration_id=DATA_CAN_ID,
|
||||||
|
data=chunk,
|
||||||
|
is_extended_id=False
|
||||||
|
)
|
||||||
|
|
||||||
|
try:
|
||||||
|
bus.send(data_msg)
|
||||||
|
except can.CanError as e:
|
||||||
|
debug_print(f"Ошибка отправки данных: {str(e)}")
|
||||||
|
return
|
||||||
|
|
||||||
|
ack = wait_for_ack(bus)
|
||||||
|
if not ack:
|
||||||
|
debug_print("Таймаут ACK DATA")
|
||||||
|
return
|
||||||
|
|
||||||
|
# Финал
|
||||||
|
debug_print("Отправка FINISH...")
|
||||||
|
finish_msg = can.Message(
|
||||||
|
arbitration_id=BOOT_CAN_END,
|
||||||
|
data=bytes([0xAA]),
|
||||||
|
is_extended_id=False
|
||||||
|
)
|
||||||
|
bus.send(finish_msg)
|
||||||
|
|
||||||
|
ack = wait_for_ack(bus, timeout=3)
|
||||||
|
if ack and ack.data[0] == 0xAA:
|
||||||
|
debug_print("Прошивка подтверждена!")
|
||||||
|
else:
|
||||||
|
debug_print("Ошибка верификации!")
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
debug_print(f"Критическая ошибка: {str(e)}")
|
||||||
|
finally:
|
||||||
|
bus.shutdown()
|
||||||
|
|
||||||
|
def wait_for_ack(bus, timeout=1.0):
|
||||||
|
start_time = time.time()
|
||||||
|
while time.time() - start_time < timeout:
|
||||||
|
msg = bus.recv(timeout=0) # Неблокирующий режим
|
||||||
|
if msg and msg.arbitration_id == ACK_CAN_ID:
|
||||||
|
return msg
|
||||||
|
return None
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
import sys
|
||||||
|
if len(sys.argv) != 3:
|
||||||
|
print("Использование: sudo python3 can_flasher.py firmware.hex")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
send_firmware(sys.argv[1])
|
||||||
73
test/python_test_id.py
Normal file
73
test/python_test_id.py
Normal file
|
|
@ -0,0 +1,73 @@
|
||||||
|
import can
|
||||||
|
import time
|
||||||
|
|
||||||
|
def send_write_read_requests():
|
||||||
|
try:
|
||||||
|
bus = can.interface.Bus(channel='can0', bustype='socketcan')
|
||||||
|
|
||||||
|
# Конфигурация сообщений (ЗАПОЛНИТЕ ВАШИ ЗНАЧЕНИЯ)
|
||||||
|
write_msg = {
|
||||||
|
'arbitration_id': 0x01, # CAN ID для записи
|
||||||
|
'data': [0x27, 0xA0, 0xFF, 0x00], # Данные для записи (4 байта)
|
||||||
|
'description': "Установка id устройства"
|
||||||
|
}
|
||||||
|
|
||||||
|
read_msg = {
|
||||||
|
'arbitration_id': 0x01, # CAN ID для чтения
|
||||||
|
'data': [0xFF,0x99], # Адрес новый + команда запроса данных
|
||||||
|
'description': "Запрос id устройства",
|
||||||
|
'response_id': 0xFF, # Ожидаемый ID ответа
|
||||||
|
'timeout': 1.0 # Таймаут ожидания ответа (сек)
|
||||||
|
}
|
||||||
|
|
||||||
|
# 1. Отправка команды записи
|
||||||
|
print("Отправка команды записи...")
|
||||||
|
msg = can.Message(
|
||||||
|
arbitration_id=write_msg['arbitration_id'],
|
||||||
|
data=write_msg['data'],
|
||||||
|
is_extended_id=False
|
||||||
|
)
|
||||||
|
bus.send(msg)
|
||||||
|
print(f"Запись: ID={hex(msg.arbitration_id)}, Данные={list(msg.data)}")
|
||||||
|
|
||||||
|
# Ждем обработки команды устройством
|
||||||
|
time.sleep(2.0)
|
||||||
|
|
||||||
|
# 2. Отправка запроса чтения и ожидание ответа
|
||||||
|
print("\nОтправка запроса чтения...")
|
||||||
|
msg = can.Message(
|
||||||
|
arbitration_id=read_msg['arbitration_id'],
|
||||||
|
data=read_msg['data'],
|
||||||
|
is_extended_id=False
|
||||||
|
)
|
||||||
|
bus.send(msg)
|
||||||
|
print(f"Чтение: ID={hex(msg.arbitration_id)}, Команда={list(msg.data)}")
|
||||||
|
|
||||||
|
# Ожидаем ответа
|
||||||
|
start_time = time.time()
|
||||||
|
response_received = False
|
||||||
|
|
||||||
|
print("\nОжидание ответа...")
|
||||||
|
while (time.time() - start_time) < read_msg['timeout']:
|
||||||
|
response = bus.recv(timeout=0.1)
|
||||||
|
|
||||||
|
if response and response.arbitration_id == read_msg['response_id']:
|
||||||
|
print(f"\nПолучен ответ: ID={hex(response.arbitration_id)}")
|
||||||
|
print(f"Данные: {list(response.data)}")
|
||||||
|
print(f"Длина: {response.dlc} байт")
|
||||||
|
response_received = True
|
||||||
|
break
|
||||||
|
|
||||||
|
if not response_received:
|
||||||
|
print("\nОшибка: ответ не получен в течение заданного времени")
|
||||||
|
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("\nПрерывание пользователем")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"Ошибка: {str(e)}")
|
||||||
|
finally:
|
||||||
|
bus.shutdown()
|
||||||
|
print("\nCAN соединение закрыто")
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
send_write_read_requests()
|
||||||
35
test/send_velocity_impulses.py
Normal file
35
test/send_velocity_impulses.py
Normal file
|
|
@ -0,0 +1,35 @@
|
||||||
|
import can
|
||||||
|
import struct
|
||||||
|
import time
|
||||||
|
|
||||||
|
# Function to send the target speed
|
||||||
|
def send_target_speed(bus, target_speed):
|
||||||
|
msg = can.Message()
|
||||||
|
msg.arbitration_id = 1 # Message ID
|
||||||
|
msg.is_extended_id = False
|
||||||
|
msg.dlc = 5 # Message length
|
||||||
|
msg.data = [ord('V')] + list(struct.pack('<f', target_speed)) # 'V' for the command identifier, followed by the speed in float format
|
||||||
|
|
||||||
|
try:
|
||||||
|
bus.send(msg)
|
||||||
|
print(f"Sent message with target speed: {target_speed} m/s")
|
||||||
|
print(f"Message data: {msg.data}")
|
||||||
|
except can.CanError:
|
||||||
|
print("Message failed to send")
|
||||||
|
|
||||||
|
# Main function
|
||||||
|
def main():
|
||||||
|
# CAN interface setup
|
||||||
|
bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings
|
||||||
|
print("CAN bus initialized, sending target speed impulses...")
|
||||||
|
|
||||||
|
# Send impulses of target speed from -2 to 2 m/s
|
||||||
|
target_speeds = [-1, 1]
|
||||||
|
|
||||||
|
while True:
|
||||||
|
for speed in target_speeds:
|
||||||
|
send_target_speed(bus, speed)
|
||||||
|
time.sleep(1) # 1-second delay between messages
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
Loading…
Add table
Add a link
Reference in a new issue