samples: added motor control playground samples

Signed-off-by: Felipe Neves <[email protected]>

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)

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.

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

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";
+	};
+};

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";
+	};
+};
+

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"

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));
+}

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
+
+