diff --git a/samples/motion_control_playground/CMakeLists.txt b/samples/motion_control_playground/CMakeLists.txt new file mode 100644 index 0000000..e3a7077 --- /dev/null +++ b/samples/motion_control_playground/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 3.13.1) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(zephyr_motor_controller) + +target_sources(app PRIVATE src/main.c) +target_sources(app PRIVATE src/motor_control_pipeline.c) +target_sources(app PRIVATE src/adrc_control_law.c) +target_sources(app PRIVATE src/pid_control_law.c) diff --git a/samples/motion_control_playground/README.rst b/samples/motion_control_playground/README.rst new file mode 100644 index 0000000..7d06e11 --- /dev/null +++ b/samples/motion_control_playground/README.rst @@ -0,0 +1,98 @@ +Servo Control system demonstration +********************************** + +This sample presents an application to perform a servo +control by setting a target position and letting an CAN +based servo motor to track it, additionally it offers +PID and ADRC controllers ready to use, tune and live +load. + +The graphical view of the system can be done using STMViewer +since the initial target board is a ST Nucleo G474RE which +has a CAN controller. + + +Requirements +************ + +Hardware +======== +* MF4005 CAN Bus servo motor (Make sure to checkout a CAN Bus version not the RS485): + * https://www.aliexpress.com/w/wholesale-mf4005.html?spm=a2g0o.detail.search.0 + * http://en.lkmotor.cn + +* ST Nucleo G474RE: + * https://www.st.com/en/evaluation-tools/nucleo-g474re.html + +* A CAN Bus transceiver (A 3.3V): + * https://www.aliexpress.com/item/1005003450161614.html?channel=twinner + +The Motor can be supplied by a regular 12 VDC power supply, its CANH and CANL +wires should be attached to the CANH and CANL side of the CAN transceiver board +while the CAN TX and CAN RX should be wired to the nucleo board: +* PA11 -> CAN RX; + +* PA12 -> CAN TX; + +These signals can be found in the Board's CN10 connector on the top side. + + +Firmware +======== + +* Install STMViewer if you would like to see the graphics while tunning (support for ST SoC only): + * https://github.com/klonyyy/STMViewer/releases + +* Clone this repository. + +Building and Flashing +********************* + +Using this sample with the supported G474RE Nucleo board does not +require special instructions, just use west command as shown below: + +```$ west build -p always -b nucleo_g474re samples/motor_control_playground``` + +Flashing also does not require special steps, just type: + +```$ west flash``` + +Expected result +*************** + +After flashing open your favorite terminal and connect to the Board +you might able to see the Zephyr console and some commands can be +executed (refer main.c to check how the shell commands are implemented): + +``` +uart:~$ zephyr_motor_control se + set_reference set_gains_pid set_gains_adrc +uart:~$ zephyr_motor_control set_reference 270 +uart:~$ zephyr_motor_control set_reference 45 +uart:~$ zephyr_motor_control set_reference 90 +uart:~$ zephyr_motor_control set_reference 120 +uart:~$ zephyr_motor_control set_reference 0 +uart:~$ zephyr_motor_control set_reference 10 +uart:~$ zephyr_motor_control set_reference 200 +uart:~$ zephyr_motor_control set_gains_adrc 0.0005 200 0.1 10 0.2 + +``` + +The demonstration firmware implements a digital generic control loop +which receives an interface to the motor plant, the interface supports +other implementations beyond the MF4005, check the implementation and +adapt to your motor. + +The generic control loop can receive dynamically a control law object +that takes a form of an generic control law interface, and it was +extended to implement both PID and ADRC controllers, other controllers +like LQR or MPC can be implmented under this interface, check both +pid_control_law.c and adrc_control_law.c implementations. + +Additionally there are commands on shell to load ADRC or PID controllers +on thecontrol loop and commands to set their gains, with help of +STMViewer the user can perform the control loops real time, graphical +tunning, saving hours of debug. + +The current instances are tuned for the MF4005 plant and it is stable offering +also good tracking and almost zero overshoot under step response. diff --git a/samples/motion_control_playground/boards/arduino_giga_r1_stm32h747xx_m7.conf b/samples/motion_control_playground/boards/arduino_giga_r1_stm32h747xx_m7.conf new file mode 100644 index 0000000..58522db --- /dev/null +++ b/samples/motion_control_playground/boards/arduino_giga_r1_stm32h747xx_m7.conf @@ -0,0 +1,7 @@ +# Copyright (c) 2024 Felipe Neves +# SPDX-License-Identifier: Apache-2.0 + +CONFIG_USB_DEVICE_STACK=y +CONFIG_USB_DEVICE_INITIALIZE_AT_BOOT=y +CONFIG_USB_WORKQUEUE_STACK_SIZE=16384 +CONFIG_MAIN_STACK_SIZE=8912 diff --git a/samples/motion_control_playground/boards/arduino_giga_r1_stm32h747xx_m7.overlay b/samples/motion_control_playground/boards/arduino_giga_r1_stm32h747xx_m7.overlay new file mode 100644 index 0000000..529e70e --- /dev/null +++ b/samples/motion_control_playground/boards/arduino_giga_r1_stm32h747xx_m7.overlay @@ -0,0 +1,61 @@ +/* + * Copyright (c) 2024 Felipe Neves + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/ { + chosen { + zephyr,console = &cdc_acm_uart0; + zephyr,shell-uart = &cdc_acm_uart0; + zephyr,cdc-acm-uart0 = &cdc_acm_uart0; + }; +}; + +&fdcan2 { + bus-speed = <1000000>; + status = "okay"; + + #address-cells = <1>; + #size-cells = <0>; + mf4005_motor0: mf4005_motor@141 { + compatible = "lkm,mf4005"; + reg = <0x141>; + max-current-ma = <240>; + encoder-resolution-bits = <16>; + max-speed-dps = <12000>; + status = "okay"; + }; + + mf4005_motor1: mf4005_motor@142 { + compatible = "lkm,mf4005"; + reg = <0x142>; + max-current-ma = <240>; + encoder-resolution-bits = <16>; + max-speed-dps = <12000>; + status = "okay"; + }; +}; + +zephyr_udc0: &usbotg_fs { + pinctrl-0 = <&usb_otg_fs_dm_pa11 &usb_otg_fs_dp_pa12>; + pinctrl-names = "default"; + status = "okay"; + + cdc_acm_uart0: cdc_acm_uart0 { + compatible = "zephyr,cdc-acm-uart"; + }; +}; + +&cdc_acm_uart0 { + status = "okay"; +}; + +&timers2 { + st,prescaler = <1>; + status = "okay"; + + counter { + status = "okay"; + }; +}; diff --git a/samples/motion_control_playground/boards/nucleo_g474re.overlay b/samples/motion_control_playground/boards/nucleo_g474re.overlay new file mode 100644 index 0000000..828f607 --- /dev/null +++ b/samples/motion_control_playground/boards/nucleo_g474re.overlay @@ -0,0 +1,42 @@ +/* + * Copyright (c) 2024 Felipe Neves + * + * SPDX-License-Identifier: Apache-2.0 + */ + +&fdcan1 { + pinctrl-0 = <&fdcan1_rx_pa11 &fdcan1_tx_pa12>; + pinctrl-names = "default"; + status = "okay"; + bus-speed = <1000000>; + + #address-cells = <1>; + #size-cells = <0>; + mf4005_motor0: mf4005_motor@141 { + compatible = "lkm,mf4005"; + reg = <0x141>; + max-current-ma = <240>; + encoder-resolution-bits = <16>; + max-speed-dps = <12000>; + status = "okay"; + }; + + mf4005_motor1: mf4005_motor@142 { + compatible = "lkm,mf4005"; + reg = <0x142>; + max-current-ma = <240>; + encoder-resolution-bits = <16>; + max-speed-dps = <12000>; + status = "okay"; + }; +}; + +&timers2 { + st,prescaler = <1>; + status = "okay"; + + counter { + status = "okay"; + }; +}; + diff --git a/samples/motion_control_playground/prj.conf b/samples/motion_control_playground/prj.conf new file mode 100644 index 0000000..cca557c --- /dev/null +++ b/samples/motion_control_playground/prj.conf @@ -0,0 +1,16 @@ +CONFIG_PRINTK=y +CONFIG_SERIAL=y +CONFIG_CBPRINTF_FP_SUPPORT=y + +CONFIG_SHELL=y +CONFIG_SHELL_HISTORY=y +CONFIG_SHELL_VT100_COLORS=y +CONFIG_COUNTER=y + +CONFIG_SYSTEM_WORKQUEUE_STACK_SIZE=8192 +CONFIG_SYSTEM_WORKQUEUE_PRIORITY=-4 + +CONFIG_CAN=y +CONFIG_LKMOTOR_MF4005_DRIVER=y +CONFIG_MF4005_DRIVER_SHELL=y +CONFIG_KERNEL_BIN_NAME="motor_control_playground" \ No newline at end of file diff --git a/samples/motion_control_playground/src/adrc_control_law.c b/samples/motion_control_playground/src/adrc_control_law.c new file mode 100644 index 0000000..503df01 --- /dev/null +++ b/samples/motion_control_playground/src/adrc_control_law.c @@ -0,0 +1,100 @@ +/* + * Copyright (c) 2024 Felipe Neves + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "adrc_control_law.h" + +static int reset (struct siso_control_law * self) +{ + struct adrc_control_law *al = + CL_CONTAINER_OF(self, struct adrc_control_law, interface); + + al->interface.reference = 0.0f; + al->interface.measurement = 0.0f; + al->zhat[0] = 0.0f; + al->zhat[1] = 0.0f; + al->zhat[2] = 0.0f; + al->u_prev = 0.0f; + + return 0; +} + +static int set (struct siso_control_law * self, float reference, float measurement) +{ + struct adrc_control_law *al = + CL_CONTAINER_OF(self, struct adrc_control_law, interface); + + al->interface.reference = reference; + al->interface.measurement = measurement; + + return 0; +} + +static int update (struct siso_control_law * self, float dt, float *command) +{ + struct adrc_control_law *al = + CL_CONTAINER_OF(self, struct adrc_control_law, interface); + + if(!command) + return -EINVAL; + + float ref = al->interface.reference; + float mes = al->interface.measurement; + float zhat_1 = al->zhat[0]; + float zhat_2 = al->zhat[1]; + float zhat_3 = al->zhat[2]; + float l1 = al->l[0]; + float l2 = al->l[1]; + float l3 = al->l[2]; + float kp = al->kp; + float kd = al->kd; + float b0 = al->b0; + float u_prev = al->u_prev; + + //First do the observer dynamics: + float zhat_dot_1 = -l1 * zhat_1 + zhat_2 + l1 * mes; + float zhat_dot_2 = -l2 * zhat_1 + zhat_3 + l2 * mes + b0 * u_prev; + float zhat_dot_3 = -l3 * zhat_1 + l3 * mes; + + //Now update the estimated states: + zhat_1 += zhat_dot_1 * dt; + zhat_2 += zhat_dot_2 * dt; + zhat_3 += zhat_dot_3 * dt; + + //and finally compute the control law: + float u = (kp * (ref - zhat_1) - kd * zhat_2 - zhat_3) / b0; + + al->u_prev = u; + al->zhat[0] = zhat_1; + al->zhat[1] = zhat_2; + al->zhat[2] = zhat_3; + + *command = u; + + return 0; +} + +int adrc_control_law_tune(struct adrc_control_law *al, float dt, float wo, float b0, float kp, float kd) +{ + if(!al) + return -EINVAL; + + al->interface.reset = reset; + al->interface.set = set; + al->interface.update = update; + al->b0 = b0; + al->kp = kp; + al->kd = kd; + + //Compute discrete time observer gains: + float z_eso = exp(-wo * dt); + float one_minus_zeso = (1 - z_eso); + + al->l[2] = 1 - (z_eso * z_eso * z_eso); + al->l[1] = (3.0f /(2 * dt)) * (one_minus_zeso * one_minus_zeso) * (1 + z_eso); + al->l[0] = (1 / ( dt * dt)) * (one_minus_zeso * one_minus_zeso * one_minus_zeso * one_minus_zeso); + + return (control_law_reset(&al->interface)); +} \ No newline at end of file diff --git a/samples/motion_control_playground/src/adrc_control_law.h b/samples/motion_control_playground/src/adrc_control_law.h new file mode 100644 index 0000000..7af0bd4 --- /dev/null +++ b/samples/motion_control_playground/src/adrc_control_law.h @@ -0,0 +1,35 @@ +/* + * Copyright (c) 2024 Felipe Neves + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __ADRC_CONTROL_LAW_H +#define __ADRC_CONTROL_LAW_H + +#include "control_law.h" + +struct adrc_control_law { + struct siso_control_law interface; + float b0; + float kp; + float kd; + float l[3]; + float zhat[3]; + float u_prev; +}; + +static inline struct siso_control_law * get_adrc_control_law_interface(struct adrc_control_law *al) +{ + if(al) { + return &al->interface; + } + + return NULL; +} + +int adrc_control_law_tune(struct adrc_control_law *al, float dt, float wo, float b0, float kp, float kd); + +#endif + + diff --git a/samples/motion_control_playground/src/control_law.h b/samples/motion_control_playground/src/control_law.h new file mode 100644 index 0000000..481aa97 --- /dev/null +++ b/samples/motion_control_playground/src/control_law.h @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2024 Felipe Neves + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __CONTROL_LAW_H +#define __CONTROL_LAW_H + +#include +#include +#include + +//Abstract control Law class and methods: +struct siso_control_law { + int (*reset)(struct siso_control_law * self); + int (*set)(struct siso_control_law * self, float reference, float measurement); + int (*update)(struct siso_control_law * self, float dt, float *command); + + float error; + float measurement; + float reference; +}; + +static inline int control_law_reset(struct siso_control_law *cl) +{ + if(!cl) + return -EINVAL; + + if(cl->reset) { + return cl->reset(cl); + } + + return -ENOTSUP; +} + +static inline int control_law_set(struct siso_control_law *cl, float reference, float measurement) +{ + if(!cl) + return -EINVAL; + + if(cl->set) { + return cl->set(cl, reference, measurement); + } + + return -ENOTSUP; +} + +static inline float control_law_update(struct siso_control_law *cl, float dt, float *command) +{ + if(!cl) + return -EINVAL; + + if(cl->update) { + return cl->update(cl, dt, command); + } + + return -ENOTSUP; +} + + +//extract derived control law class from this base class: +#define CL_CONTAINER_OF(ptr, type, field) ((type *)(((char *)(ptr)) - offsetof(type, field))) + +#endif \ No newline at end of file diff --git a/samples/motion_control_playground/src/main.c b/samples/motion_control_playground/src/main.c new file mode 100644 index 0000000..c2f8eb5 --- /dev/null +++ b/samples/motion_control_playground/src/main.c @@ -0,0 +1,121 @@ +/* + * Copyright (c) 2024 Felipe Neves + * + * SPDX-License-Identifier: Apache-2.0 + */ +#include +#include +#include +#include + +#include "control_law.h" +#include "adrc_control_law.h" +#include "pid_control_law.h" +#include "motor_control_pipeline.h" + +static const struct device * motor_1 = DEVICE_DT_GET(DT_NODELABEL(mf4005_motor0)); +static struct motor_control_pipeline control_1; +static struct adrc_control_law motor_adrc_1; +static struct pid_control_law motor_pid_1; + +int main(void) +{ + pid_control_law_tune(&motor_pid_1, 1.0f, 0.0f, 0.0f, 36000); + adrc_control_law_tune(&motor_adrc_1, 0.0005f, 300.0f, 1.0f, 10.0f, 0.001f); + motor_control_pipeline_add_hw(&control_1, motor_1); + motor_control_pipeline_add_control(&control_1, get_adrc_control_law_interface(&motor_adrc_1)); + motor_control_pipeline_register(&control_1, 1); + motor_control_pipeline_set_position(&control_1, 90); + + return 0; +} + +static int motor_set_ref(const struct shell *shell, size_t argc, char **argv) +{ + if (argc != 2) { + return -EINVAL; + } + + return motor_control_pipeline_set_position(&control_1, (strtof(argv[1], NULL))); +} + +static int motor_gen_ramp(const struct shell *shell, size_t argc, char **argv) +{ + if (argc != 3) { + return -EINVAL; + } + + float start = strtof(argv[1], NULL); + float end = strtof(argv[2], NULL); + float current = start; + float steps = (end - start) / 16.0f; + + for(; current < end; current += steps) { + motor_control_pipeline_set_position(&control_1, current); + k_sleep(K_MSEC(250)); + } + + return 0; + +} + +static int motor_change_control(const struct shell *shell, size_t argc, char **argv) +{ + if (argc != 2) { + return -EINVAL; + } + + long option = strtol(argv[1], NULL, 10); + + if(!option) { + motor_control_pipeline_add_control(&control_1, get_pid_control_law_interface(&motor_pid_1)); + } else { + motor_control_pipeline_add_control(&control_1, get_adrc_control_law_interface(&motor_adrc_1)); + } + + return 0; +} + +static int motor_set_gains_pid(const struct shell *shell, size_t argc, char **argv) +{ + if (argc != 5) { + return -EINVAL; + } + + pid_control_law_tune(&motor_pid_1, + (strtof(argv[1], NULL)), + (strtof(argv[2], NULL)), + (strtof(argv[3], NULL)), + (strtof(argv[4], NULL))); + + return 0; + +} + +static int motor_set_gains_adrc(const struct shell *shell, size_t argc, char **argv) +{ + if (argc != 6) { + return -EINVAL; + } + + adrc_control_law_tune(&motor_adrc_1, + (strtof(argv[1], NULL)), + (strtof(argv[2], NULL)), + (strtof(argv[3], NULL)), + (strtof(argv[4], NULL)), + (strtof(argv[5], NULL))); + + return 0; +} + +SHELL_STATIC_SUBCMD_SET_CREATE( + motor_control, + SHELL_CMD(generate_test_ramp, NULL, "Generate an up and down ramp of 5 seconds", motor_gen_ramp), + SHELL_CMD(set_reference, NULL, "Set velocity reference in RPM", motor_set_ref), + SHELL_CMD(change_control_algo, NULL, "0 - PID, 1 - ADRC", motor_change_control), + SHELL_CMD(set_gains_pid, NULL, "Set velocity PID KP and KI", motor_set_gains_pid), + SHELL_CMD(set_gains_adrc, NULL, "Set velocity ADRC Gains", motor_set_gains_adrc), + SHELL_SUBCMD_SET_END + ); + +SHELL_CMD_REGISTER(zephyr_motor_control, &motor_control, "Zephyr RTOS Motor control commands", NULL); diff --git a/samples/motion_control_playground/src/motor_control_pipeline.c b/samples/motion_control_playground/src/motor_control_pipeline.c new file mode 100644 index 0000000..70383e0 --- /dev/null +++ b/samples/motion_control_playground/src/motor_control_pipeline.c @@ -0,0 +1,141 @@ +#include +#include +#include +#include +#include +#include "motor_control_pipeline.h" + +#define CONTROL_PIPELINE_US_PERIOD (500) + +static struct motor_control_pipeline* pipeline; +static float now_seconds = 0.0f; +static const struct device *timer_dev = DEVICE_DT_GET( DT_INST(0,st_stm32_counter)); +static struct counter_alarm_cfg alarm_cfg; + +static void motor_control_work_handler(struct k_work *work) +{ + uint32_t now_ticks; + counter_get_value(timer_dev, &now_ticks); + now_seconds = (float)(counter_ticks_to_us(timer_dev, now_ticks)) * 1e-6f; + + if(pipeline) { + int64_t chan_value; + float last_time = pipeline->last_time; + float target_position = pipeline->target_position; + float current_position; + float control_effort; + int ticks = pipeline->ticks; + int sample_ratio = pipeline->sample_ratio; + int err, werr; + + ticks++; + if(ticks == sample_ratio) { + ticks = 0; + float dt = fabs(now_seconds - last_time); + + err = mf4005_motor_channel_get(pipeline->hw, MF4005_CHAN_POSITION_MILI_DEGREES, &chan_value); + current_position = (float)chan_value * 0.001f; + + if(pipeline->position_cl) { + control_law_set(pipeline->position_cl, target_position, current_position); + control_law_update(pipeline->position_cl, dt, &control_effort); + } else { + control_effort = target_position; + } + + werr = mf4005_motor_channel_set(pipeline->hw, MF4005_CHAN_SPEED_DEG_SECONDS, (int)control_effort); + + pipeline->control_effort = control_effort; + pipeline->current_position = current_position; + pipeline->last_time = now_seconds; + pipeline->dt = dt; + pipeline->reading_error = err; + pipeline->writing_error = werr; + } + pipeline->ticks = ticks; + } +} + +K_WORK_DEFINE(motor_work, motor_control_work_handler); + +static void timer_interrupt_fn(const struct device *counter_dev, + uint8_t chan_id, uint32_t ticks, + void *user_data) +{ + k_work_submit(&motor_work); + counter_set_channel_alarm(timer_dev, 0, &alarm_cfg); +} + +static int motor_control_pipeline_init_backend(void) +{ + counter_start(timer_dev); + alarm_cfg.flags = 0; + alarm_cfg.ticks = counter_us_to_ticks(timer_dev, CONTROL_PIPELINE_US_PERIOD); + alarm_cfg.callback = timer_interrupt_fn; + alarm_cfg.user_data = &alarm_cfg; + counter_set_channel_alarm(timer_dev, 0, &alarm_cfg); + + return 0; +} + +SYS_INIT(motor_control_pipeline_init_backend, POST_KERNEL, CONFIG_APPLICATION_INIT_PRIORITY); + +int motor_control_pipeline_add_hw(struct motor_control_pipeline* cp, const struct device *hw) +{ + if(!cp || !hw) + return -EINVAL; + + cp->hw = hw; + + return 0; +} + +int motor_control_pipeline_add_control(struct motor_control_pipeline* cp, struct siso_control_law *cl) +{ + if(!cp) + return -EINVAL; + + cp->position_cl = cl; + + return 0; +} + +int motor_control_pipeline_remove_control(struct motor_control_pipeline* cp) +{ + if(!cp) + return -EINVAL; + + cp = NULL; + + return 0; +} + +int motor_control_pipeline_register(struct motor_control_pipeline* cp, int sample_ratio) +{ + if(!cp) + return -EINVAL; + + if(!cp->hw) + return -ENODEV; + + if(!sample_ratio) + return -EINVAL; + + cp->sample_ratio = sample_ratio; + cp->ticks = 0; + cp->last_time = 0.0f; + + pipeline = cp; + + return 0; +} + +int motor_control_pipeline_set_position(struct motor_control_pipeline* cp, float target_position) +{ + if(!cp) + return -EINVAL; + + cp->target_position = target_position; + + return 0; +} diff --git a/samples/motion_control_playground/src/motor_control_pipeline.h b/samples/motion_control_playground/src/motor_control_pipeline.h new file mode 100644 index 0000000..87ca9ff --- /dev/null +++ b/samples/motion_control_playground/src/motor_control_pipeline.h @@ -0,0 +1,29 @@ +#ifndef __MOTOR_CONTROL_PIPELINE_H +#define __MOTOR_CONTROL_PIPELINE_H + +#include +#include +#include +#include "control_law.h" + +struct motor_control_pipeline { + const struct device *hw; + struct siso_control_law *position_cl; + float last_time; + float dt; + float target_position; + float current_position; + float control_effort; + int ticks; + int sample_ratio; + int reading_error; + int writing_error; +}; + +int motor_control_pipeline_add_hw(struct motor_control_pipeline* cp, const struct device *hw); +int motor_control_pipeline_add_control(struct motor_control_pipeline* cp, struct siso_control_law *cl); +int motor_control_pipeline_remove_control(struct motor_control_pipeline* cp); +int motor_control_pipeline_set_position(struct motor_control_pipeline* cp, float target_position); +int motor_control_pipeline_register(struct motor_control_pipeline* cp, int sample_ratio); + +#endif diff --git a/samples/motion_control_playground/src/pid_control_law.c b/samples/motion_control_playground/src/pid_control_law.c new file mode 100644 index 0000000..12c8b43 --- /dev/null +++ b/samples/motion_control_playground/src/pid_control_law.c @@ -0,0 +1,82 @@ +#include "pid_control_law.h" + +static int reset (struct siso_control_law * self) +{ + struct pid_control_law *pl = + CL_CONTAINER_OF(self, struct pid_control_law, interface); + + pl->integrator = 0.0f; + pl->prev_error = 0.0f; + pl->interface.reference = 0.0f; + pl->interface.measurement = 0.0f; + + return 0; +} + +static int set (struct siso_control_law * self, float reference, float measurement) +{ + struct pid_control_law *pl = + CL_CONTAINER_OF(self, struct pid_control_law, interface); + + pl->interface.reference = reference; + pl->interface.measurement = measurement; + return 0; +} + +static int update (struct siso_control_law * self, float dt, float *command) +{ + (void)dt; + + struct pid_control_law *pl = + CL_CONTAINER_OF(self, struct pid_control_law, interface); + + if(!command) + return -EINVAL; + + float ref = pl->interface.reference; + float mes = pl->interface.measurement; + float kp = pl->kp; + float ki = pl->ki; + float kd = pl->kd; + float integrator = pl->integrator; + float prev_error = pl->prev_error; + float max_integrator = pl->max_integrator; + + float error = ref - mes; + float error_derivative = error - prev_error; + + integrator += error; + if(integrator > max_integrator) { + integrator = max_integrator; + } else if ( integrator < -max_integrator) { + integrator = -max_integrator; + } + + float u = (kp * error) + (ki * integrator) + (kd * error_derivative); + + prev_error = error; + + pl->integrator = integrator; + pl->prev_error = prev_error; + pl->max_integrator = max_integrator; + *command = u; + + return 0; +} + +int pid_control_law_tune(struct pid_control_law *pl, float kp, float ki, float kd, float max_integrator) +{ + if(!pl) + return -EINVAL; + + pl->interface.reset = reset; + pl->interface.set = set; + pl->interface.update = update; + + pl->kp = kp; + pl->ki = ki; + pl->kd = kd; + pl->max_integrator = max_integrator; + + return (control_law_reset(&pl->interface)); +} \ No newline at end of file diff --git a/samples/motion_control_playground/src/pid_control_law.h b/samples/motion_control_playground/src/pid_control_law.h new file mode 100644 index 0000000..375cf60 --- /dev/null +++ b/samples/motion_control_playground/src/pid_control_law.h @@ -0,0 +1,29 @@ +#ifndef __PID_CONTROL_LAW_H +#define __PID_CONTROL_LAW_H + +#include "control_law.h" + +struct pid_control_law { + struct siso_control_law interface; + float kp; + float ki; + float kd; + float integrator; + float prev_error; + float max_integrator; +}; + +static inline struct siso_control_law * get_pid_control_law_interface(struct pid_control_law *pl) +{ + if(pl) { + return &pl->interface; + } + + return NULL; +} + +int pid_control_law_tune(struct pid_control_law *pl, float kp, float ki, float kd, float max_integrator); + +#endif + +