Add FOC motor controller example
For now, only open-loop velocity control is supported.
This commit is contained in:
raiden00pl
Xiang Xiao
parent
d75d6b7af0
commit
864a61a431
@ -361,6 +361,11 @@ Dependencies:
|
||||
- `CONFIG_BUILD_PROTECTED=n` and `CONFIG_BUILD_KERNEL=n` – This test uses
|
||||
internal OS interfaces and so is not available in the NUTTX kernel builds.
|
||||
|
||||
## `foc` FOC motor controller
|
||||
|
||||
A FOC motor controller based on the NuttX FOC driver and the NuttX FOC library.
|
||||
See `apps/foc/README.md` for more information.
|
||||
|
||||
## `flowc` Serial Hardware Flow Control
|
||||
|
||||
A simple test of serial hardware flow control.
|
||||
|
||||
236
examples/foc/Kconfig
Normal file
236
examples/foc/Kconfig
Normal file
@ -0,0 +1,236 @@
|
||||
#
|
||||
# For a description of the syntax of this configuration file,
|
||||
# see the file kconfig-language.txt in the NuttX tools repository.
|
||||
#
|
||||
|
||||
menuconfig EXAMPLES_FOC
|
||||
tristate "FOC motor controller example"
|
||||
depends on MOTOR_FOC
|
||||
depends on INDUSTRY_FOC
|
||||
---help---
|
||||
Enable the FOC motor controller example.
|
||||
At the moment, this example implements a simple open-loop velocity controller.
|
||||
|
||||
if EXAMPLES_FOC
|
||||
|
||||
config EXAMPLES_FOC_DEVPATH
|
||||
string "FOC device path prefix"
|
||||
default "/dev/foc"
|
||||
---help---
|
||||
The default path to the FOC device without the device minor number.
|
||||
Default: /dev/foc
|
||||
|
||||
config EXAMPLES_FOC_FLOAT_INST
|
||||
int "FOC float instances"
|
||||
depends on INDUSTRY_FOC_FLOAT
|
||||
default 0
|
||||
|
||||
config EXAMPLES_FOC_FIXED16_INST
|
||||
int "FOC fixed16 instances"
|
||||
depends on INDUSTRY_FOC_FIXED16
|
||||
default 0
|
||||
|
||||
config EXAMPLES_FOC_CONTROL_PRIO
|
||||
int "FOC control thread priority"
|
||||
default 255
|
||||
|
||||
config EXAMPLES_FOC_CONTROL_STACKSIZE
|
||||
int "FOC control thread stack size"
|
||||
default 4096
|
||||
|
||||
config EXAMPLES_FOC_VERBOSE
|
||||
int "Enable verbose print for app"
|
||||
default 1
|
||||
range 0 2
|
||||
|
||||
config EXAMPLES_FOC_PWM_FREQ
|
||||
int "FOC PWM frequency"
|
||||
default 10000
|
||||
---help---
|
||||
Select the FOC PWM switching frequency
|
||||
|
||||
config EXAMPLES_FOC_NOTIFIER_FREQ
|
||||
int "FOC notifier frequency"
|
||||
default EXAMPLES_FOC_PWM_FREQ
|
||||
---help---
|
||||
Select the FOC notifier frequency
|
||||
|
||||
config EXAMPLES_FOC_IPHASE_ADC
|
||||
int "FOC phase current scale [x100000]"
|
||||
default 0
|
||||
---help---
|
||||
This parameter is used to get real currents from ADC RAW values
|
||||
|
||||
config EXAMPLES_FOC_STATE_PRINT_FREQ
|
||||
int "FOC example data printer frequency"
|
||||
default 0
|
||||
depends on INDUSTRY_FOC_HANDLER_PRINT
|
||||
---help---
|
||||
Set 0 to disable FOC data print
|
||||
|
||||
config EXAMPLES_FOC_STATE_USE_MODEL_PMSM
|
||||
bool "FOC uses PMSM model"
|
||||
depends on INDUSTRY_FOC_MODEL_PMSM
|
||||
default n
|
||||
---help---
|
||||
Use PMSM model instead of real hardware
|
||||
|
||||
menu "FOC user input"
|
||||
|
||||
config EXAMPLES_FOC_HAVE_ADC
|
||||
bool
|
||||
default n
|
||||
|
||||
choice
|
||||
prompt "FOC VBUS source"
|
||||
default EXAMPLES_FOC_VBUS_CONST
|
||||
|
||||
config EXAMPLES_FOC_VBUS_CONST
|
||||
bool "Use hardcoded constant VBUS value"
|
||||
|
||||
config EXAMPLES_FOC_VBUS_ADC
|
||||
bool "Use VBUS provided by ADC interface"
|
||||
depends on ADC
|
||||
select EXAMPLES_FOC_HAVE_ADC
|
||||
|
||||
endchoice # FOC VBUS interface
|
||||
|
||||
if EXAMPLES_FOC_VBUS_CONST
|
||||
|
||||
config EXAMPLES_FOC_VBUS_CONST_VALUE
|
||||
int "FOC VBUS constant value"
|
||||
default 12000
|
||||
|
||||
endif # EXAMPLES_FOC_VBUS_CONST
|
||||
|
||||
if EXAMPLES_FOC_HAVE_ADC
|
||||
|
||||
config EXAMPLES_FOC_ADC_DEVPATH
|
||||
string "FOC ADC interface path"
|
||||
default "/dev/adc0"
|
||||
|
||||
config EXAMPLES_FOC_ADC_VREF
|
||||
int "FOC ADC reference votlage [x1000]"
|
||||
default 0
|
||||
|
||||
config EXAMPLES_FOC_ADC_MAX
|
||||
int "FOC ADC aux maximum sample value"
|
||||
default 0
|
||||
|
||||
endif # EXAMPLES_FOC_HAVE_ADC
|
||||
|
||||
if EXAMPLES_FOC_VBUS_ADC
|
||||
|
||||
config EXAMPLES_FOC_VBUS_SCALE
|
||||
int "FOC VBUS SCALE scale [x1000]"
|
||||
default 0
|
||||
|
||||
endif # EXAMPLES_FOC_VBUS_ADC
|
||||
|
||||
choice
|
||||
prompt "FOC velocity source"
|
||||
default EXAMPLES_FOC_VEL_CONST
|
||||
|
||||
config EXAMPLES_FOC_VEL_CONST
|
||||
bool "Use hardcoded constant velocity value"
|
||||
|
||||
config EXAMPLES_FOC_VEL_ADC
|
||||
bool "Use ADC to control velocity"
|
||||
select EXAMPLES_FOC_HAVE_ADC
|
||||
|
||||
endchoice # FOC velocity interface
|
||||
|
||||
if EXAMPLES_FOC_VEL_CONST
|
||||
|
||||
config EXAMPLES_FOC_VEL_CONST_VALUE
|
||||
int "FOC hardoced velocity value"
|
||||
default 100000
|
||||
|
||||
endif # EXAMPLES_FOC_VEL_CONST
|
||||
|
||||
if EXAMPLES_FOC_VEL_ADC
|
||||
|
||||
config EXAMPLES_FOC_VEL_ADC_MAX
|
||||
int "FOC maximum velocity from ADC [x1000]"
|
||||
default 100000
|
||||
|
||||
endif # EXAMPLES_FOC_VEL_ADC
|
||||
|
||||
config EXAMPLES_FOC_TIME_DEFAULT
|
||||
int "FOC run time default (sec)"
|
||||
default 10
|
||||
|
||||
config EXAMPLES_FOC_STATE_INIT
|
||||
int "FOC motor controller state init"
|
||||
default 1
|
||||
range 1 4
|
||||
---help---
|
||||
1 - motor FREE (no current)
|
||||
2 - motor STOP (active break)
|
||||
3 - motor moves in CW direction
|
||||
4 - motor moves in CCW direction
|
||||
|
||||
config EXAMPLES_FOC_HAVE_BUTTON
|
||||
bool "FOC button support"
|
||||
default n
|
||||
---help---
|
||||
The button is used to change the motor controller state
|
||||
|
||||
if EXAMPLES_FOC_HAVE_BUTTON
|
||||
|
||||
config EXAMPLES_FOC_BUTTON_DEVPATH
|
||||
string "FOC button device path"
|
||||
default "/dev/buttons"
|
||||
depends on INPUT_BUTTONS
|
||||
|
||||
endif
|
||||
|
||||
endmenu # FOC user input
|
||||
|
||||
menu "FOC controller parameters"
|
||||
|
||||
config EXAMPLES_FOC_OPMODE
|
||||
int "FOC operation mode"
|
||||
default 2
|
||||
range 1 3
|
||||
---help---
|
||||
1 - IDLE mode
|
||||
2 - voltage open-loop velocity controller (default)
|
||||
3 - current open-loop velocity controller
|
||||
|
||||
config EXAMPLES_FOC_OPENLOOP_Q
|
||||
int "FOC open-loop Vq/Iq setting [x1000]"
|
||||
default 200
|
||||
|
||||
config EXAMPLES_FOC_IDQ_KP
|
||||
int "FOC PI controller Kp gain [x1000]"
|
||||
default 0
|
||||
---help---
|
||||
It is set to 0 by default and must be properly configured by the user!
|
||||
The vaule of Kp and Ki depends on the controlled motor parameters.
|
||||
For more instructions see README.md for this example.
|
||||
|
||||
config EXAMPLES_FOC_IDQ_KI
|
||||
int "FOC PI controller Ki gain [x1000]"
|
||||
default 0
|
||||
---help---
|
||||
It is set to 0 by default and must be properly configured by the user!
|
||||
The vaule of Kp and Ki depends on the controlled motor parameters.
|
||||
For more instructions see README.md for this example.
|
||||
|
||||
config EXAMPLES_FOC_RAMP_THR
|
||||
int "FOC velocity ramp threshold [x1000]"
|
||||
default 0
|
||||
|
||||
config EXAMPLES_FOC_RAMP_ACC
|
||||
int "FOC velocity ramp acc [x1000]"
|
||||
default 0
|
||||
|
||||
config EXAMPLES_FOC_RAMP_DEC
|
||||
int "FOC velocity ramp acc [x1000]"
|
||||
default 0
|
||||
|
||||
endmenu # FOC controller parameters
|
||||
|
||||
endif # EXAMPLES_FOC
|
||||
|
||||
24
examples/foc/Make.defs
Normal file
24
examples/foc/Make.defs
Normal file
@ -0,0 +1,24 @@
|
||||
############################################################################
|
||||
# apps/examples/foc/Make.defs
|
||||
# Adds selected applications to apps/ build
|
||||
#
|
||||
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
# contributor license agreements. See the NOTICE file distributed with
|
||||
# this work for additional information regarding copyright ownership. The
|
||||
# ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
# "License"); you may not use this file except in compliance with the
|
||||
# License. You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
# License for the specific language governing permissions and limitations
|
||||
# under the License.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
ifeq ($(CONFIG_EXAMPLES_FOC),y)
|
||||
CONFIGURED_APPS += examples/foc
|
||||
endif
|
||||
49
examples/foc/Makefile
Normal file
49
examples/foc/Makefile
Normal file
@ -0,0 +1,49 @@
|
||||
############################################################################
|
||||
# apps/examples/foc/Makefile
|
||||
#
|
||||
# Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
# contributor license agreements. See the NOTICE file distributed with
|
||||
# this work for additional information regarding copyright ownership. The
|
||||
# ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
# "License"); you may not use this file except in compliance with the
|
||||
# License. You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
# License for the specific language governing permissions and limitations
|
||||
# under the License.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
include $(APPDIR)/Make.defs
|
||||
|
||||
# FOC built-in application info
|
||||
|
||||
PROGNAME = foc
|
||||
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||
STACKSIZE = $(CONFIG_DEFAULT_TASK_STACKSIZE)
|
||||
MODULE = $(CONFIG_EXAMPLES_FOC)
|
||||
|
||||
# FOC motor controller example
|
||||
|
||||
MAINSRC = foc_main.c
|
||||
|
||||
ASRCS =
|
||||
CSRCS = foc_device.c foc_mq.c
|
||||
|
||||
# fixed16 support
|
||||
|
||||
ifeq ($(CONFIG_INDUSTRY_FOC_FIXED16),y)
|
||||
CSRCS += foc_fixed16_thr.c
|
||||
endif
|
||||
|
||||
# float32 support
|
||||
|
||||
ifeq ($(CONFIG_INDUSTRY_FOC_FLOAT),y)
|
||||
CSRCS += foc_float_thr.c
|
||||
endif
|
||||
|
||||
include $(APPDIR)/Application.mk
|
||||
83
examples/foc/README.md
Normal file
83
examples/foc/README.md
Normal file
@ -0,0 +1,83 @@
|
||||
# FOC example
|
||||
|
||||
The main purpose of this example is to provide a universal template to
|
||||
implement the motor controller based on the kernel-side FOC device and
|
||||
the application-side FOC library.
|
||||
|
||||
At the moment, this example implements a simple open-loop velocity controller.
|
||||
|
||||
# Hardware setup
|
||||
|
||||
This example has not yet implemented any mechanism to protect the
|
||||
powered device. This means that there is no overtemeprature
|
||||
protection, no overcurrent protection and no overvoltage protection.
|
||||
|
||||
Make sure that you power the device properly and provide current
|
||||
limits on your own so as not to break your hardware.
|
||||
|
||||
# Configuration
|
||||
|
||||
The FOC PI current controller parameters can be obtained from the given
|
||||
equations:
|
||||
|
||||
```
|
||||
Kp = ccb * Ls;
|
||||
pp = Rs / Ls;
|
||||
Ki = pp * Kp * T;
|
||||
```
|
||||
|
||||
where:
|
||||
Kp - PI proportional coefficient
|
||||
Ki - PI integral coefficient
|
||||
Rs - average phase serial resistance
|
||||
Ls - average phase serial inductance
|
||||
pp - pole plant
|
||||
ccb - current control bandwidth
|
||||
T - sampling period
|
||||
|
||||
## Sample parameters for some commercially available motors
|
||||
|
||||
* Odrive D6374 150KV
|
||||
p = 7
|
||||
Rs = 0.0254 Ohm
|
||||
Ls = 8.73 uH
|
||||
i\_max = ?
|
||||
v\_max = ?
|
||||
|
||||
Example configuration for f\_PWM = 20kHz, f\_notifier = 10kHz, ccb=1000:
|
||||
Kp = 0.0087
|
||||
Ki = 0.0025
|
||||
|
||||
* Linix 45ZWN24-40 (PMSM motor dedicated for NXP FRDM-MC-LVMTR kit)
|
||||
p = 2
|
||||
Rs = 0.5 Ohm
|
||||
Ls = 0.400 mH
|
||||
i\_max = 2.34 A
|
||||
v\_max = 24 V
|
||||
|
||||
Example configuration for f\_PWM = 10kHz, f\_notifier = 5kHz, ccb=1000:
|
||||
Kp = 0.4
|
||||
Ki = 0.1
|
||||
|
||||
* Bull-Running BR2804-1700 kV (motor provided with the ST P-NUCLEO-IHM07 kit)
|
||||
p = 7
|
||||
Rs = 0.11 Ohm
|
||||
Ls = 0.018 mH
|
||||
i\_max = 1.2A
|
||||
v\_max = 12V
|
||||
|
||||
Example configuration for f\_PWM = 20kHz, f\_notifier = 10kHz, ccb=200:
|
||||
Kp = 0.036
|
||||
Ki = 0.022
|
||||
|
||||
* iPower GBM2804H-100T (gimbal motor provided with the ST P-NUCLEO-IHM03 kit)
|
||||
p = 7
|
||||
Rs = 5.29 Ohm
|
||||
Ls = 1.05 mH
|
||||
i\_max = 0.15A
|
||||
v\_max = 12V
|
||||
|
||||
Example configuration for f\_PWM = 10kHz, f\_notifier = 5kHz, ccb=TODO:
|
||||
Kp = TODO
|
||||
Ki = TODO
|
||||
|
||||
119
examples/foc/foc_adc.h
Normal file
119
examples/foc/foc_adc.h
Normal file
@ -0,0 +1,119 @@
|
||||
/****************************************************************************
|
||||
* apps/examples/foc/foc_adc.h
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __EXAMPLES_FOC_FOC_ADC_H
|
||||
#define __EXAMPLES_FOC_FOC_ADC_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* VBUS source must be specified */
|
||||
|
||||
#if defined(CONFIG_EXAMPLES_FOC_VBUS_CONST) && \
|
||||
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
|
||||
# error
|
||||
#endif
|
||||
|
||||
/* Velocity source must be specified */
|
||||
|
||||
#if defined(CONFIG_EXAMPLES_FOC_VEL_CONST) && \
|
||||
defined(CONFIG_EXAMPLES_FOC_VEL_ADC)
|
||||
# error
|
||||
#endif
|
||||
|
||||
/* VBUS ADC scale factor */
|
||||
|
||||
#ifdef CONFIG_EXAMPLES_FOC_VBUS_ADC
|
||||
# define VBUS_ADC_SCALE (CONFIG_EXAMPLES_FOC_ADC_VREF * \
|
||||
CONFIG_EXAMPLES_FOC_VBUS_SCALE / \
|
||||
CONFIG_EXAMPLES_FOC_ADC_MAX / \
|
||||
1000.0f / \
|
||||
1000.0f)
|
||||
#endif
|
||||
|
||||
/* Velocity ADC scale factor */
|
||||
|
||||
#ifdef CONFIG_EXAMPLES_FOC_VEL_ADC
|
||||
# define VEL_ADC_SCALE (1.0f / CONFIG_EXAMPLES_FOC_ADC_MAX)
|
||||
#endif
|
||||
|
||||
/* If constant velocity is selected, velocity value must be provided */
|
||||
|
||||
#ifdef CONFIG_EXAMPLES_FOC_VEL_CONST
|
||||
# define VEL_ADC_SCALE (1)
|
||||
# if CONFIG_EXAMPLES_FOC_VEL_CONST_VALUE == 0
|
||||
# error
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/* If constant VBUS is selected, VBUS value must be provided */
|
||||
|
||||
#ifdef CONFIG_EXAMPLES_FOC_VBUS_CONST
|
||||
# define VBUS_ADC_SCALE (1)
|
||||
# define VBUS_CONST_VALUE (CONFIG_EXAMPLES_FOC_VBUS_CONST_VALUE / 1000.0f)
|
||||
# if CONFIG_EXAMPLES_FOC_VBUS_CONST_VALUE == 0
|
||||
# error
|
||||
# endif
|
||||
#endif
|
||||
|
||||
/* Additional configuration if ADC support is required */
|
||||
|
||||
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
|
||||
|
||||
/* AUX ADC samples support */
|
||||
|
||||
# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \
|
||||
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
|
||||
# define ADC_SAMPLES (2)
|
||||
# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC) || \
|
||||
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
|
||||
# define ADC_SAMPLES (1)
|
||||
# else
|
||||
# define ADC_SAMPLES (0)
|
||||
#endif
|
||||
|
||||
/* Verify ADC FIFO size */
|
||||
|
||||
# if CONFIG_ADC_FIFOSIZE != (ADC_SAMPLES + 1)
|
||||
# error Invalid ADC FIFO size
|
||||
# endif
|
||||
|
||||
/* Numerate ADC samples */
|
||||
|
||||
# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \
|
||||
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
|
||||
# define VBUS_ADC_SAMPLE (0)
|
||||
# define VEL_ADC_SAMPLE (1)
|
||||
# elif defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
|
||||
# define VBUS_ADC_SAMPLE (0)
|
||||
# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC)
|
||||
# define VEL_ADC_SAMPLE (0)
|
||||
# endif
|
||||
|
||||
#endif /* CONFIG_EXAMPLES_FOC_HAVE_ADC */
|
||||
|
||||
#endif /* __EXAMPLES_FOC_FOC_ADC_H */
|
||||
82
examples/foc/foc_cfg.h
Normal file
82
examples/foc/foc_cfg.h
Normal file
@ -0,0 +1,82 @@
|
||||
/****************************************************************************
|
||||
* apps/examples/foc/foc_cfg.h
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __EXAMPLES_FOC_FOC_CFG_H
|
||||
#define __EXAMPLES_FOC_FOC_CFG_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* Velocity ramp must be configured */
|
||||
|
||||
#if (CONFIG_EXAMPLES_FOC_RAMP_THR == 0)
|
||||
# error
|
||||
#endif
|
||||
#if (CONFIG_EXAMPLES_FOC_RAMP_ACC == 0)
|
||||
# error
|
||||
#endif
|
||||
#if (CONFIG_EXAMPLES_FOC_RAMP_DEC == 0)
|
||||
# error
|
||||
#endif
|
||||
|
||||
/* ADC Iphase ratio must be provided */
|
||||
|
||||
#if (CONFIG_EXAMPLES_FOC_IPHASE_ADC == 0)
|
||||
# error
|
||||
#endif
|
||||
|
||||
/* Printer prescaler */
|
||||
|
||||
#if defined(CONFIG_INDUSTRY_FOC_HANDLER_PRINT) && \
|
||||
(CONFIG_EXAMPLES_FOC_STATE_PRINT_FREQ > 0)
|
||||
# define FOC_STATE_PRINT_PRE (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ / \
|
||||
CONFIG_EXAMPLES_FOC_STATE_PRINT_FREQ)
|
||||
#else
|
||||
# undef FOC_STATE_PRINT_PRE
|
||||
#endif
|
||||
|
||||
/* Velocity ramp configuration */
|
||||
|
||||
#define RAMP_CFG_THR (CONFIG_EXAMPLES_FOC_RAMP_THR / 1000.0f)
|
||||
#define RAMP_CFG_ACC (CONFIG_EXAMPLES_FOC_RAMP_ACC / 1000.0f)
|
||||
#define RAMP_CFG_DEC (CONFIG_EXAMPLES_FOC_RAMP_DEC / 1000.0f)
|
||||
|
||||
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
|
||||
|
||||
/* PMSM model parameters */
|
||||
|
||||
# define FOC_MODEL_POLES 7
|
||||
# define FOC_MODEL_LOAD (1.0f)
|
||||
# define FOC_MODEL_RES (0.11f)
|
||||
# define FOC_MODEL_IND (0.0002f)
|
||||
# define FOC_MODEL_INER (0.1f)
|
||||
# define FOC_MODEL_FLUX (0.001f)
|
||||
# define FOC_MODEL_INDD (0.0002f)
|
||||
# define FOC_MODEL_INDQ (0.0002f)
|
||||
#endif
|
||||
|
||||
#endif /* __EXAMPLES_FOC_FOC_CFG_H */
|
||||
60
examples/foc/foc_debug.h
Normal file
60
examples/foc/foc_debug.h
Normal file
@ -0,0 +1,60 @@
|
||||
/****************************************************************************
|
||||
* apps/examples/foc/foc_debug.h
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __EXAMPLES_FOC_FOC_DEBUG_H
|
||||
#define __EXAMPLES_FOC_FOC_DEBUG_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* Print support */
|
||||
|
||||
#if CONFIG_EXAMPLES_FOC_VERBOSE == 0
|
||||
# define PRINTF(...)
|
||||
# define PRINTFV(...)
|
||||
#elif CONFIG_EXAMPLES_FOC_VERBOSE == 1
|
||||
# define PRINTF(format, ...) printf(format, ##__VA_ARGS__)
|
||||
# define PRINTFV(...)
|
||||
#else
|
||||
# define PRINTF(format, ...) printf(format, ##__VA_ARGS__)
|
||||
# define PRINTFV(format, ...) printf(format, ##__VA_ARGS__)
|
||||
#endif
|
||||
|
||||
/* If print is enabled we need float print support */
|
||||
|
||||
#if CONFIG_EXAMPLES_FOC_VERBOSE > 1
|
||||
# ifdef CONFIG_INDUSTRY_FOC_FLOAT
|
||||
# ifndef CONFIG_LIBC_FLOATINGPOINT
|
||||
# error "CONFIG_LIBC_FLOATINGPOINT must be set!"
|
||||
# endif
|
||||
# endif
|
||||
#endif
|
||||
|
||||
#endif /* __EXAMPLES_FOC_FOC_DEBUG_H */
|
||||
82
examples/foc/foc_device.c
Normal file
82
examples/foc/foc_device.c
Normal file
@ -0,0 +1,82 @@
|
||||
/****************************************************************************
|
||||
* apps/examples/foc/foc_device.c
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include "foc_debug.h"
|
||||
#include "foc_device.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_device_open
|
||||
****************************************************************************/
|
||||
|
||||
int foc_device_open(FAR struct foc_device_s *dev, int id)
|
||||
{
|
||||
char devpath[32];
|
||||
int ret = OK;
|
||||
|
||||
DEBUGASSERT(dev);
|
||||
|
||||
/* Get FOC devpath */
|
||||
|
||||
sprintf(devpath, "%s%d", CONFIG_EXAMPLES_FOC_DEVPATH, id);
|
||||
|
||||
/* Open FOC device */
|
||||
|
||||
dev->fd = open(devpath, 0);
|
||||
if (dev->fd <= 0)
|
||||
{
|
||||
PRINTF("ERROR: open %s failed %d\n", devpath, errno);
|
||||
ret = -errno;
|
||||
goto errout;
|
||||
}
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_device_close
|
||||
****************************************************************************/
|
||||
|
||||
int foc_device_close(FAR struct foc_device_s *dev)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
DEBUGASSERT(dev);
|
||||
|
||||
if (dev->fd > 0)
|
||||
{
|
||||
close(dev->fd);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
52
examples/foc/foc_device.h
Normal file
52
examples/foc/foc_device.h
Normal file
@ -0,0 +1,52 @@
|
||||
/****************************************************************************
|
||||
* apps/examples/foc/foc_device.h
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __EXAMPLES_FOC_FOC_DEVICE_H
|
||||
#define __EXAMPLES_FOC_FOC_DEVICE_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definition
|
||||
****************************************************************************/
|
||||
|
||||
/* FOC device data */
|
||||
|
||||
struct foc_device_s
|
||||
{
|
||||
int fd; /* FOC device */
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
int foc_device_open(FAR struct foc_device_s *dev, int id);
|
||||
int foc_device_close(FAR struct foc_device_s *dev);
|
||||
|
||||
#endif /* __EXAMPLES_FOC_FOC_DEVICE_H */
|
||||
1018
examples/foc/foc_fixed16_thr.c
Normal file
1018
examples/foc/foc_fixed16_thr.c
Normal file
File diff suppressed because it is too large
Load Diff
1017
examples/foc/foc_float_thr.c
Normal file
1017
examples/foc/foc_float_thr.c
Normal file
File diff suppressed because it is too large
Load Diff
1118
examples/foc/foc_main.c
Normal file
1118
examples/foc/foc_main.c
Normal file
File diff suppressed because it is too large
Load Diff
118
examples/foc/foc_mq.c
Normal file
118
examples/foc/foc_mq.c
Normal file
@ -0,0 +1,118 @@
|
||||
/****************************************************************************
|
||||
* apps/examples/foc/foc_mq.c
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <errno.h>
|
||||
#include <strings.h>
|
||||
|
||||
#include "foc_debug.h"
|
||||
#include "foc_mq.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_mq_handle
|
||||
****************************************************************************/
|
||||
|
||||
int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h)
|
||||
{
|
||||
int ret = OK;
|
||||
uint8_t buffer[5];
|
||||
|
||||
/* Get data from AUX */
|
||||
|
||||
ret = mq_receive(mq, (char *)buffer, 5, 0);
|
||||
if (ret < 0)
|
||||
{
|
||||
if (errno != EAGAIN)
|
||||
{
|
||||
PRINTF("ERROR: mq_receive failed %d\n", errno);
|
||||
ret = -errno;
|
||||
goto errout;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Timeout */
|
||||
|
||||
ret = OK;
|
||||
goto errout;
|
||||
}
|
||||
}
|
||||
|
||||
/* Verify message length */
|
||||
|
||||
if (ret != 5)
|
||||
{
|
||||
PRINTF("ERROR: invalid message length = %d\n", ret);
|
||||
goto errout;
|
||||
}
|
||||
|
||||
/* Handle message */
|
||||
|
||||
switch (buffer[0])
|
||||
{
|
||||
case CONTROL_MQ_MSG_VBUS:
|
||||
{
|
||||
memcpy(&h->vbus, &buffer[1], 4);
|
||||
break;
|
||||
}
|
||||
|
||||
case CONTROL_MQ_MSG_APPSTATE:
|
||||
{
|
||||
memcpy(&h->app_state, &buffer[1], 4);
|
||||
break;
|
||||
}
|
||||
|
||||
case CONTROL_MQ_MSG_VEL:
|
||||
{
|
||||
memcpy(&h->vel, &buffer[1], 4);
|
||||
break;
|
||||
}
|
||||
|
||||
case CONTROL_MQ_MSG_START:
|
||||
{
|
||||
h->start = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case CONTROL_MQ_MSG_KILL:
|
||||
{
|
||||
h->quit = true;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
PRINTF("ERROR: invalid message type %d\n", buffer[0]);
|
||||
ret = -EINVAL;
|
||||
goto errout;
|
||||
}
|
||||
}
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
83
examples/foc/foc_mq.h
Normal file
83
examples/foc/foc_mq.h
Normal file
@ -0,0 +1,83 @@
|
||||
/****************************************************************************
|
||||
* apps/examples/foc/foc_mq.h
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __EXAMPLES_FOC_FOC_MQ_H
|
||||
#define __EXAMPLES_FOC_FOC_MQ_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <mqueue.h>
|
||||
|
||||
#include "foc_device.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define CONTROL_MQ_MAXMSG (10)
|
||||
#define CONTROL_MQ_MSGSIZE (5)
|
||||
#define CONTROL_MQ_MQNAME "mqf"
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definition
|
||||
****************************************************************************/
|
||||
|
||||
/* Thread message type */
|
||||
|
||||
enum foc_thr_msg_e
|
||||
{
|
||||
CONTROL_MQ_MSG_INVALID = 0,
|
||||
CONTROL_MQ_MSG_VBUS = 1,
|
||||
CONTROL_MQ_MSG_APPSTATE = 2,
|
||||
CONTROL_MQ_MSG_VEL = 3,
|
||||
CONTROL_MQ_MSG_START = 4,
|
||||
CONTROL_MQ_MSG_KILL = 5
|
||||
};
|
||||
|
||||
/* FOC thread handler */
|
||||
|
||||
struct foc_mq_s
|
||||
{
|
||||
bool quit;
|
||||
bool start;
|
||||
int app_state;
|
||||
uint32_t vbus;
|
||||
uint32_t vel;
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_mq_handle
|
||||
****************************************************************************/
|
||||
|
||||
int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h);
|
||||
|
||||
#endif /* __EXAMPLES_FOC_FOC_THR_H */
|
||||
101
examples/foc/foc_thr.h
Normal file
101
examples/foc/foc_thr.h
Normal file
@ -0,0 +1,101 @@
|
||||
/****************************************************************************
|
||||
* apps/examples/foc/foc_thr.h
|
||||
*
|
||||
* Licensed to the Apache Software Foundation (ASF) under one or more
|
||||
* contributor license agreements. See the NOTICE file distributed with
|
||||
* this work for additional information regarding copyright ownership. The
|
||||
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
||||
* "License"); you may not use this file except in compliance with the
|
||||
* License. You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
* License for the specific language governing permissions and limitations
|
||||
* under the License.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __EXAMPLES_FOC_FOC_THR_H
|
||||
#define __EXAMPLES_FOC_FOC_THR_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <nuttx/motor/foc/foc.h>
|
||||
|
||||
#include <mqueue.h>
|
||||
|
||||
#include "foc_device.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definition
|
||||
****************************************************************************/
|
||||
|
||||
/* FOC example state */
|
||||
|
||||
enum foc_example_state_e
|
||||
{
|
||||
FOC_EXAMPLE_STATE_INVALID = 0, /* Reserved */
|
||||
FOC_EXAMPLE_STATE_FREE = 1, /* No current */
|
||||
FOC_EXAMPLE_STATE_STOP = 2, /* Active break */
|
||||
FOC_EXAMPLE_STATE_CW = 3, /* CW direction */
|
||||
FOC_EXAMPLE_STATE_CCW = 4, /* CCW direction */
|
||||
};
|
||||
|
||||
/* Operation modes */
|
||||
|
||||
enum foc_operation_mode_e
|
||||
{
|
||||
FOC_OPMODE_INVALID = 0, /* Reserved */
|
||||
FOC_OPMODE_IDLE = 1, /* IDLE */
|
||||
FOC_OPMODE_OL_V_VEL = 2, /* Voltage open-loop velocity controller */
|
||||
FOC_OPMODE_OL_C_VEL = 3, /* Current open-loop velocity controller */
|
||||
|
||||
/* Not supported yet */
|
||||
|
||||
#if 0
|
||||
FOC_OPMODE_CL_C_TRQ = 3, /* Current closed-loop torque controller */
|
||||
FOC_OPMODE_CL_C_VEL = 4, /* Current closed-loop velocity controller */
|
||||
FOC_OPMODE_CL_C_POS = 5 /* Current closed-loop position controller */
|
||||
#endif
|
||||
};
|
||||
|
||||
/* FOC thread data */
|
||||
|
||||
struct foc_ctrl_env_s
|
||||
{
|
||||
struct foc_device_s dev; /* FOC device */
|
||||
mqd_t mqd; /* Control msg queue */
|
||||
int id; /* FOC device id */
|
||||
int inst; /* Type specific instance counter */
|
||||
int type; /* Controller type */
|
||||
int qparam; /* Open-loop Q setting (x1000) */
|
||||
int mode; /* Operation mode */
|
||||
uint32_t pi_kp; /* FOC PI Kp (x1000) */
|
||||
uint32_t pi_ki; /* FOC PI Ki (x1000) */
|
||||
uint32_t velmax; /* Velocity max (x1000) */
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_FLOAT
|
||||
int foc_float_thr(FAR struct foc_ctrl_env_s *envp);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_FIXED16
|
||||
int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp);
|
||||
#endif
|
||||
|
||||
#endif /* __EXAMPLES_FOC_FOC_THR_H */
|
||||
Loading…
x
Reference in New Issue
Block a user